Support for chainable wrench / torque command controller

[mhubii]

Attempts to fix #115 / #250 by turning LBRWrenchCommandController into a ChainableControllerInterface. Also see https://control.ros.org/rolling/doc/ros2_control/controller_manager/doc/controller_chaining.html.

Same might be done for LBRTorqueCommandController while at it.

Some thoughts:

• Requires documentation of Document load data calibration #235
  • Update fri Java app to include tools
  • Throw on FRI 2.x until supported in Java app
• Consider using external force state interface for load data safety checking (i.e. expect 0 external force on initialization), see
  

  lbr_fri_ros2_stack/lbr_description/ros2_control/lbr_system_config.yaml

  Line 26 in 693d77f

  estimated_ft_sensor: # estimates the external force-torque from the external joint torque values

  
  This would also force users to enable the FT estimation as interfaces will not be available otherwise.
  • In compliant control modes, system inteface should check (true value is only available in COMMANDING_ACTIVE state, realistically can only check in CommandGuard / new StateGuard? for external torques as external forces computation would slow the interfaces. Maybe in on state switch to guarantee single check during initialization?)
  • In position control modes, compliant controller should check (add this to wrench / torque command controller and admittance controller)
• Add configurable maximum wrenches (cartesian impedance) and joint torques (impedance)
• Add a demo for use with MoveIt / just the joint trajectory controller. Notes:
  • Managed to run join trajectory controller, interfaces are e.g. named lbr_wrench_command_controller/lbr_A1
  • MoveIt struggles to command joints that do not exist inside URDF. Unsure how to combine MoveIt with chained
    controller
    Mabye replace this moveit_controller_manager
    

    lbr_fri_ros2_stack/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml

    Line 3 in 693d77f

    moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

    
    with moveit_controller_manager: moveit_ros_control_interface/MoveItControllerManager . Potentially even try use Ros2ControllerManager https://github.com/moveit/moveit2/blob/89acaddda936efd4acdade37578bee6ce037f66b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp#L133
• Update topic names (by IDL names):
  • wrench -> lbr_wrench_command
  • torque -> lbr_torque_command
  • joint_position -> lbr_joint_position_command (for consitency)

[Copilot]

Pull Request Overview

This PR adds chainable controller support to the LBRWrenchCommandController, enabling the controller to operate in chained mode with updated state and command interface management.

• Inherits from ChainableControllerInterface and updates the export functions for state and command interfaces.
• Introduces new subscriptions for wrench command handling along with corresponding reset and initialization methods.
• Updates pluginlib export to use the new chainable interface.

Reviewed Changes

Copilot reviewed 2 out of 3 changed files in this pull request and generated no comments.

File	Description
lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp	Updates include new subscription logic, refined state and command interface exports, reset functions, and chainable mode handling.
lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp	Header modifications to reflect the chainable interface inheritance, new function prototypes, and additional include directives.

Files not reviewed (1)

• lbr_ros2_control/plugin_description_files/controllers.xml: Language not supported

Comments suppressed due to low confidence (1)

lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp:270

• The implementation of reset functions appears to be inverted: 'reset_lbr_wrench_command_subscription_()' resets 'wrench_command_subscription_ptr_' instead of 'lbr_wrench_command_subscription_ptr_'. Consider swapping the implementations of reset_lbr_wrench_command_subscription_() and reset_wrench_command_subscription_() to ensure each resets the appropriate subscription.

void LBRWrenchCommandController::reset_lbr_wrench_command_subscription_() { if (wrench_command_subscription_ptr_) { wrench_command_subscription_ptr_.reset(); } }

[mhubii]

For future reference: Command guards / state guards should be simple instances that are instantiated from a yaml configuration and stored as a map or similar. They should be iterated over and be called on, e.g. on enter commanding active, on command, on monitor, ...

It is however still not obvious how workspace monitoring could be implemented on this low level.

Also, it should be double checked whether this functionality is redundant with what is already available in ros2_control.