Allow Hardware Interface To Access Params From ROS2 Controllers

[swiz23]

Is your feature request related to a problem? Please describe.
Hi, I'm currently working on a project containing a robot and a custom hardware interface plugin. The way the robot works - you must command a robot-specific control mode to the robot before sending down commands. For example, if you want to send down positions to all joints, you send a command to the robot to get into its 'position control' mode. If you want to send down positions to some joints and velocities to other joints, you send a command to the robot to put it into a 'combo control' mode, etc...

Until now, I've coded the custom hardware interface to smartly determine what robot-specific control mode it should be sending down to the robot by way of the 'command interfaces' param that can be specified for a JTC controller . During controller activation/deactivation, the prepare_command_mode_switch and perform_command_mode_switch functions are called in the hardware interface and the desired command interfaces to start and stop are passed in as arguments. So if all the joints are only claiming the 'position' interface, then I know to command the robot to the robot-specific 'position' control mode. If a subset of the joints are claiming the 'velocity' interface, and a different subset is claiming the 'position' interface, I know I need to command the robot to be in a 'combo control' mode.

However, I've now started a part of the project where we have two different types of 'position' control modes that can be sent down to the robot. So my previous approach of determining what control mode to send to the robot based on the 'command_interfaces' breaks down here. Should the first position control mode or the second position control mode be used?

So I guess my question is...what have other people done to solve this issue? Or in general, how have other people coded their hardware interfaces to change their robot-specific control modes?

Describe the solution you'd like
The solution I propose is to add an optional 'data' field that can be configured within a JTC (or any other) controller in a ros2_controllers.yaml file. An example can be seen below.

  trajectory_controller:
    ros__parameters:
      command_interfaces:
        - position
      state_interfaces:
        - position
        - velocity
      joints:
        - joint_1
        - joint_2
        - joint_3
        - joint_4
        - joint_5
        - joint_6
      allow_partial_joints_goal: true
      open_loop_control: true
      constraints:
        stopped_velocity_tolerance: 0.1
        goal_time: 0.0
        joint_1: { trajectory: 0.3, goal: 0.05 }
        joint_2: { trajectory: 0.3, goal: 0.05 }
        joint_3: { trajectory: 0.3, goal: 0.05 }
        joint_4: { trajectory: 0.3, goal: 0.05 }
        joint_5: { trajectory: 0.3, goal: 0.05 }
        joint_6: { trajectory: 0.3, goal: 0.05 }
     command_data: position_control_mode_2

This command_data parameter will be sent down to the prepare_command_mode_switch and perform_command_mode_switch functions within the hardware interface (along with the 'start_interfaces' and 'stop_interfaces'). It will also be a vector of strings where each string corresponds to a joint. Then within the function, the hardware interface can check the value of 'command_data' and command the appropriate control mode accordingly.

For example, in the scenario above, the following would be passed down to the hardware interface if there are no currently active controllers and you are starting the trajectory_controller above:

start_interfaces

• joint_1/position
• joint_2/position
• joint_3/position
• joint_4/position
• joint_5/position
• joint_6/position

stop_interfaces
[empty]

command_data

• joint_1/position_control_mode_2
• joint_2/position_control_mode_2
• joint_3/position_control_mode_2
• joint_4/position_control_mode_2
• joint_5/position_control_mode_2
• joint_6/position_control_mode_2

Next, in the ros2_control.xacro file, you may have a bunch of params that are prefixed with "position_control_mode_1" or maybe "position_control_mode_2", etc... Maybe you store data in these params that you want your hardware interface to command down to the hardware when doing a mode switch (ex. control mode, stiffness, damping, torque adjustments, etc...). In this manner, an end-user has much more flexibility on how their robots function.

Also note that his approach would be backwards compatible. The system_interface.cpp and actuator_interface.cpp will have 2 different versions of the 'perform_command_mode_switch' and 'prepare_command_mode_switch' functions: the current one and the one with a third argument called 'command_data'. Within the system_interface.cpp and actuator_interface.cpp files, the code checks to see if command_data is empty. If it is, the regular function signature is used (which ensures backwards compatibility). If command_data is not empty, then the new function signature is used.

Describe alternatives you've considered

• None

Additional context
The issue at ros-controls/ros2_control#347 seems somewhat related to this question, so I'm linking to it here.

[gavanderhoorn]

So I guess my question is...what have other people done to solve this issue? Or in general, how have other people coded their hardware interfaces to change their robot-specific control modes?

I've mostly seen solutions where a dedicated interface is created (in the hardware interface) which exposes a way for a dedicated "coordination controller" to configure system modes (that's not necessarily the best name). That controller then offers a ROS API to other nodes which they can use to perform mode configuration when needed.

This keeps responsibilities separated (coordination/configuration doesn't seem like a job for a trajectory controller fi), doesn't need any customisation to controllers (so you can reuse all controllers available without needing to maintain custom forks) and seems to align with the general idea of ros(2)_control to abstract access to hardware resources/interfaces. It would also potentially allow reuse of this part of the infrastructure with/by other ros2_control integrations -- although that will depend a bit on how generic you can make it.

[swiz23]

Thanks @gavanderhoorn, can you share a link to an example where this 'dedicated interface' approach was implemented?

[gavanderhoorn]

I'm afraid not. The examples I was thinking of are all private systems.

[swiz23]

Having this 'data' param though could be helpful for this PR: moveit/moveit_task_constructor#355. In that PR, a feature is added to allow you to use MTC to change controllers for a particular stage. I'm not completely sure how using the 'coordination controller' would fit into that PR (how would a higher level node know when to change the system mode?). But with the 'data' param, this issue would never surface.

[swiz23]

I've mostly seen solutions where a dedicated interface is created (in the hardware interface) which exposes a way for a dedicated "coordination controller" to configure system modes (that's not necessarily the best name)

What type of 'dedicated interface' would be created in the hardware interface? Are you saying that you create a custom 'command interface' called 'system_mode' or similar? So the hardware interface will need to be modified to not only expect 'position', 'velocity', or 'acceleration' command interfaces, but also a 'system_mode' command interface?

[destogl]

I am usually solving this by adding interfaces for each control mode, in your case that would be: position, special_position, velocity.

Then you always know in your HW interface which mode you have to switch to.

Here might be the limitation JTC if you want to use it in all three modes since we are fixing it to position, velocity, acceleration and effort interfaces. Nevertheless, a simple extension of it would do the job. For example, adding support also for custom position interfaces. (But are you certain that you need for custom position interface actually JTC?)

[swiz23]

Hi @destogl,

Thanks for the feedback!

I did think of your approach originally (but slightly after creating this Issue - thus, the reason it's not in the 'Describe Alternatives you Considered' section). Also yes - I am allowing JTC to claim more than one command interface.

That said, your response raises the question of how the command_interfaces are designed to be used. There are 2 approaches I can see:

1. The command interfaces (i.e. position, velocity, acceleration, effort) specify the type of command to be sent down to the hardware, but not the system specific control mode directly. The hardware interface then determines the system specific control mode to command the robot based on the combination (or lack thereof) of claimed command interfaces.
2. A single command interface (i.e. either position OR velocity OR acceleration OR effort OR some custom command interface) specifies the system specific control mode to be sent down to the hardware. In this setup, it doesn't make sense for a controller to claim more than one command interface at a time since each command interface is linked to a system specific control mode (this seems to be your approach).

From the fact that it's possible to have a controller claim multiple command interfaces, this led me to believe the first approach above is more in the spirit of ros2_control. As a result, it does not make sense to me to have a 1:1 relationship between command interface and the system control mode.

[destogl]

We are using the second approach right now. For your concrete example, it might be that we need some new parameters in JTC to enable different control modes from the same type of controller, i.e. JTC.