Help understanding the control stack

[Manouselis]

Hi everyone. For the past few months, I've been working on a (custom) blue ROV heavy and have been trying to use and adapt the blue/angler framework.

I've written a .cpp script (named reference_publisher) that calculates the position error and then forwards a scaled position error to the default integral sliding mode (velocity) controller. In the simulation, I configured a DVL with noise that is equal to the specified Waterlinked A50 DVL noise. I fuse this DVL with a barometer and an IMU - again with proper noise - to generate an odom topic. This cascaded controller works very well in simulation, managing to reach the desired waypoint.

However, when I try to test on the actual blue ROV, my actual ROV ends up doing circles around its center of mass, instead of moving to the desired waypoint. Note that this only happens when I use the blue/angler framework. When I actually use a joystick (and switch my SERVO parameters back to MOTOR from RC PASSTHROUGH), then the ROV works properly.

I am assuming that the issue is caused by wrong ArduSUB parameters.

The scripts I am running are:

ros2 launch blue_demos bluerov2_heavy_demo.launch.yaml
ros2 launch blue_demos bluerov2_heavy_controllers.launch.py
ros2 launch waterlinked_dvl_driver dvl.launch.py
ros2 run velocity_controllers reference_controller 

Attached, you can find the reference_controller I am using reference_publisher.cpp

Any help will be greatly appreciated, as this has been blocking my progress on my work.

[evan-palmer]

Can you share the parameters file for your ROV? I suspect that the issue is with the TAM. The simulated TAM is generally different than the hardware TAM.

Here is strategy that I recommend for debugging your setup:

1. Start by disabling all controllers during, except the thruster controllers.
2. Launch the thruster controllers, and confirm that the ArduSub parameters change to RC PASSTHROUGH
3. If that works as expected, send commands individually to each thruster to document the thruster directions when sending +/- thrust to that thruster
4. Confirm that the TAM matches the observed thruster directions. If the TAM is correct, then you can enable the TAM controller. Again send small force/torque commands to confirm that the thrusters behave as expected. In the past, I wrote a small script to map joystick inputs to the TAM controller to perform this step.
5. After verifying the TAM, you can start testing/tuning the SMC

[evan-palmer]

Here are the parameters for the thruster controllers and TAM controller that I am currently using on our system:

thruster_allocation_matrix_controller:
  ros__parameters:
    thrusters:
      - thruster1_joint
      - thruster2_joint
      - thruster3_joint
      - thruster4_joint
      - thruster5_joint
      - thruster6_joint
      - thruster7_joint
      - thruster8_joint
    reference_controllers:
      - thruster1_controller
      - thruster2_controller
      - thruster3_controller
      - thruster4_controller
      - thruster5_controller
      - thruster6_controller
      - thruster7_controller
      - thruster8_controller
    tam:
      x:  [  0.707,  0.707,   0.707,  0.707,      0.0,      0.0,     0.0,     0.0]
      y:  [ -0.707,  0.707,   0.707, -0.707,      0.0,      0.0,     0.0,     0.0]
      z:  [    0.0,    0.0,     0.0,    0.0,     -1.0,      1.0,     1.0,    -1.0]
      rx: [    0.0,    0.0,     0.0,    0.0, -0.21805, -0.21805, 0.21805, 0.21805]
      ry: [    0.0,    0.0,     0.0,    0.0,    -0.12,     0.12,   -0.12,    0.12]
      rz: [-0.1888, 0.1888, -0.1888, 0.1888,      0.0,      0.0,     0.0,     0.0]

thruster1_controller:
  ros__parameters:
    thruster: thruster1_joint
    min_thrust: -20.0
    max_thrust: 20.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster2_controller:
  ros__parameters:
    thruster: thruster2_joint
    min_thrust: -20.0
    max_thrust: 20.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster3_controller:
  ros__parameters:
    thruster: thruster3_joint
    min_thrust: -20.0
    max_thrust: 20.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster4_controller:
  ros__parameters:
    thruster: thruster4_joint
    min_thrust: -20.0
    max_thrust: 20.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster5_controller:
  ros__parameters:
    thruster: thruster5_joint
    min_thrust: -30.0
    max_thrust: 30.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster6_controller:
  ros__parameters:
    thruster: thruster6_joint
    min_thrust: -30.0
    max_thrust: 30.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster7_controller:
  ros__parameters:
    thruster: thruster7_joint
    min_thrust: -30.0
    max_thrust: 30.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]

thruster8_controller:
  ros__parameters:
    thruster: thruster8_joint
    min_thrust: -30.0
    max_thrust: 30.0
    min_deadband: 1470
    max_deadband: 1530
    neutral_pwm: 1500
    thrust_curve_coefficients: [1498.0, 12.01, -0.04731, -0.002098, 0.00002251]