The output of TAM will consist entirely of NaN when the preceeded controller does not send commands for all DOFs.

[mateusmenezes95]

Issue Description

First, I want to express my sincere gratitude for making this project open-source, as it significantly benefits individuals studying ros2_control in conjunction with underwater vehicles.

I am currently working on integrating a controller I developed during my master’s program with the Blue project. Before I submit a pull request, I would appreciate the opportunity to discuss a specific point. My focus is on an Unconstrained MPC that is application-agnostic; however, I utilized the BlueROV2 as a target application during my master’s. I am developing a demonstration package in which the controller governs the BlueROV2 Light model, equipped with only six thrusters.

In this context, I want to point out that certain ROVs maintain metacentric stability in specific DOFs. Therefore, controllers doesn't need to control all DOFs of the underwater vehicle. For instance, the BlueROV2 Light version is metacentric stable in roll and pitch; consequently, controlling surge, sway, heave, and yaw suffices for its operation (it is important to emphasize that pitch control is unattainable due to the vertical distribution of the thrusters).

Upon reviewing the code, particularly the line and output below, I noticed that if any value in the reference_interface is NaN, the entire output vector (i.e., the PWM for the thrusters) also becomes NaN.

auv_controllers/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp

Line 236 in c9c58a1

const Eigen::VectorXd thrust(tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_wrench);

ubuntu:~/ws_blue$ ros2 topic echo /thruster_allocation_matrix_controller/status --once
header:
  stamp:
    sec: 111
    nanosec: 962000000
  frame_id: ''
reference_names:
- x
- y
- z
- rx
- ry
- rz
reference:
- 50.11436096767839
- 31.700807368867643
- 0.587416318347844
- .nan
- .nan
- -0.7122953916181399
output_names:
- thruster1_joint
- thruster2_joint
- thruster3_joint
- thruster4_joint
- thruster5_joint
- thruster6_joint
output:
- .nan
- .nan
- .nan
- .nan
- .nan
- .nan
time_step: 0.034
---

To address this issue, I have locally resolved it by substituting all NaN values with zero before output computation. See below the output of the status topic after the solution.

ubuntu:~/ws_blue$ ros2 topic echo /thruster_allocation_matrix_controller/status --once
header:
  stamp:
    sec: 26
    nanosec: 520000000
  frame_id: ''
reference_names:
- x
- y
- z
- rx
- ry
- rz
reference:
- 6.8919153313286365
- 5.354266333211509
- 0.8955875569600908
- 0.0
- 0.0
- -0.2866879738981165
output_names:
- thruster1_joint
- thruster2_joint
- thruster3_joint
- thruster4_joint
- thruster5_joint
- thruster6_joint
output:
- -0.923341734723141
- -3.9507143694696696
- 4.709951588479513
- 0.16410451571329737
- 0.44143708446376734
- 0.4414370844637672
time_step: 0.034
---

I want to gather the opinion of the project maintainers regarding this modification. Should you concur with my approach, I would be pleased to submit a merge request containing the solution I have developed.

Steps to Reproduce

I have modified the gz_ros2_control to accommodate my package. Currently, providing an environment replicating the issue I reported is not feasible. However, if needed, I can arrange for an environment where the behavior I am addressing can be reproduced.

See this question for the context.

Expected Behavior

The output of the thruster allocation will remain valid even in cases where a controller does not control all DOFs of the vehicle.

Error Message

Runtime Environment

https://github.com/Robotic-Decision-Making-Lab/blue/blob/main/.docker/compose/nvidia-desktop.yaml

Additional Context

Output of ros2 control list_hardware_interfaces

command interfaces
	thruster1_joint/pwm [available] [claimed]
	thruster2_joint/pwm [available] [claimed]
	thruster3_joint/pwm [available] [claimed]
	thruster4_joint/pwm [available] [claimed]
	thruster5_joint/pwm [available] [claimed]
	thruster6_joint/pwm [available] [claimed]
	thruster_1_controller/thruster1_joint/effort [available] [claimed]
	thruster_2_controller/thruster2_joint/effort [available] [claimed]
	thruster_3_controller/thruster3_joint/effort [available] [claimed]
	thruster_4_controller/thruster4_joint/effort [available] [claimed]
	thruster_5_controller/thruster5_joint/effort [available] [claimed]
	thruster_6_controller/thruster6_joint/effort [available] [claimed]
	thruster_allocation_matrix_controller/rx/effort [available] [unclaimed]
	thruster_allocation_matrix_controller/ry/effort [available] [unclaimed]
	thruster_allocation_matrix_controller/rz/effort [available] [claimed]
	thruster_allocation_matrix_controller/x/effort [available] [claimed]
	thruster_allocation_matrix_controller/y/effort [available] [claimed]
	thruster_allocation_matrix_controller/z/effort [available] [claimed]

[evan-palmer]

This is a good idea! In your proposed solution, I think that it would be worthwhile to indicate to users that some joints are not actuated by the outer-loop controller. One way to do this could be to leave the reference value as NaN in the controller state message and to add a log message indicating that NaN values will be converted. It may also be helpful to make this a configurable parameter so that users can enable/disable the functionality if there are no unactuated joints.