⚠️ Compliant control modes feedback cycle

[mhubii]

As reported in #232, the robot drifts / executes strange motions / disconnects. The source for this error is currently unclear. This behavior can be observed in humble-2.1.2 and humble-2.2.0.

To replicate:

• Run robot in CARTESIAN_IMPEDANCE_CONTOL mode
• Command robot with WRENCH
• Run driver with ros2 launch lbr_bringup hardware.launch.py model:=med7 ctrl:=lbr_wrench_command_controller
• Set open_loop: false

[mhubii]

What likely happens is the following:

• Robot is compliant under wrench / torque control
• Measured joint position is fed back as new target when open_loop: false
• Robot enters a feedback cycle

This issue also relates to #226.

Further details (edit):

If any of the lbr_ros2_control::SystemInterface provided commands are Nan, the commands get permanently initialized with the measured joint position (leading to the feedback loop):

lbr_fri_ros2_stack/lbr_fri_ros2/src/interfaces/wrench_command.cpp

Line 23 in 58ff69a

this->init_command(state);

When no one sends commands yet, the commands remain Nan:

lbr_fri_ros2_stack/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp

Line 52 in 58ff69a

return controller_interface::return_type::OK;

Hi @Hydran00, @anselmo-parnada. The issue is explained above. Thank you again for reporting. Until this is fixed, I would recommend not to use compliant control modes in combination with this stack.

[mhubii]

Still not fully fixed as control mode not always detected correctly (somehow the correct control mode only becomes available in COMMANDING_ACTIVE state, this might be KUKAs fault)...

lbr_fri_ros2_stack/lbr_fri_ros2/src/async_client.cpp

Line 74 in 8c49c3b

if (control_mode == KUKA::FRI::EControlMode::JOINT_IMP_CONTROL_MODE ||

As indicated also in their default demos:

$ ./LBRJointSineOverlayApp 
LBRJointSineOverlayClient initialized:
	joint mask: 0x8
	frequency (Hz): 0.250000
	amplitude (rad): 0.040000
	filterCoeff: 0.990000
LBRiiwaClient state changed from 0 to 1
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 1 to 2
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 2 to 3
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 3 to 4
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 4 to 2
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 2 to 0
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
martin@rvim-sie-stack:/tmp/fri/build$ ^C
$ ./LBRWrenchSineOverlayApp 
Enter LBRWrenchSineOverlay Client Application
LBRWrenchSineOverlayClient initialized:
	frequency (Hz):  X = 0.250000, Y = 0.250000
	amplitude (N):   X = 5.000000, Y = 5.000000
LBRiiwaClient state changed from 0 to 1
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 1 to 2
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 2 to 3
control mode: 0 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2
LBRiiwaClient state changed from 3 to 4
control mode: 1 reminder: POSITION_CONTROL_MODE = 0, CART_IMP_CONTROL_MODE = 1, JOINT_IMP_CONTROL_MODE = 2

[mhubii]

this issue is now fixed on the humble and rolling branches