Clean definition of mimic joints under ros2_control URDF tag

[destogl]

The feature is related to discussion in ros-controls/gazebo_ros2_control#111.

The current definition of mimic joint features in URDF is not complete.
To have all the features, we should also add the offset parameter for the mimic joint.
This lead to the following structure:

<joint name="right_finger_joint">
  <command_interface name="effort"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>
<joint name="left_finger_joint">
  <param name="mimic">right_finger_joint</param>
  <param name="multiplier">1</param>
  <param name="offset">0</param>
  <command_interface name="position"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>

The parameters under left_finger_joint could be confusing.

Describe the solution you'd like
Therefore, I propose a clearer definition for the mimic joints:

<joint name="right_finger_joint">
  <command_interface name="effort"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>
<joint name="left_finger_joint">
  <mimic joint="right_finger_joint" multiplier="1" offset="0" />
  <command_interface name="position"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>

This example uses URDF syntax (see here and search for ""-tag).

Describe alternatives you've considered
An alternative could be to use the existing definition in URDF (outside of <ros2_controlł>-tag) and mark the joint type as a mimic.
For example this could look like:

<joint name="right_finger_joint" type="prismatic">
  <axis xyz="0 1 0"/>
  <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
  <parent link="base"/>
  <child link="finger_right"/>
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<joint name="left_finger_joint" type="prismatic">
  <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
  <axis xyz="0 1 0"/>
  <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
  <parent link="base"/>
  <child link="finger_left"/>
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<ros2_control>
  <joint name="right_finger_joint">
    <command_interface name="effort"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>
  <joint name="left_finger_joint" mimic="true">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>
</ros2_control>

Or something similar.

What do you think? @bmagyar @christophfroehlich

Additional context
To implement this, the following has to be done:

1. Extension of test and example URDFs in ros2_control_test_assets. For example, by adding additional joint ("joint4") that is mimicked.
2. Extension of the component parser to support mimic joints in hardware_interface/src/component_parser.cpp.
3. The old behavior should be preserved as a fallback solution. Add "deprecation" notice to the component parser if the "mimic" parameter is defined.
4. Adjustment of FakeSystem to use this type of mimic joints. Also, the previous behavior should be preserved and deprecated.
5. Extend simulator-integrations: gazebo_ros2_control and ign_ros2_control plugins.

[christophfroehlich]

The second/alternative seems to be less redundant and still easy to understand.
But is it then necessary to explicitly set mimic to true for ros2_control? Is there a use case where one would specify a mimic joint in the original URDF, but wants this to be disabled in ros2_control?

[bmagyar]

I think it's clearer if as much as possible is is explained as part of the URDF. After all, these are properties of the kinematics structure, not control IMO. We only help making it happen in simulation.

[destogl]

@bmagyar would this mean that we should always search in URDF if there are some mimic joints? Theoretically speaking, we don't need to add a mimic joint in the control part at all. But then we are maybe missing definition of interfaces. Could you give a more concrete answer about what you are having in mind?

[bmagyar]

After a discussion with @destogl it seems we converged on a solution very close to the above.

Overview: Using a mimic joint should work out-of-the-box, on by default. Users who want to opt-in can do so in the configuration of their hardware component. The mimic property should be defined in the wider URDF section, not under the <ros2_control> tag.

In the example with the setup below, ros2_control should do the check on each joint to figure out if it is a mimic joint or not. This could be part of JointInfo (will need to reach up to the full urdf to find it) ((implementation detail))

<joint name="right_finger_joint" type="prismatic">
  <axis xyz="0 1 0"/>
  <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
  <parent link="base"/>
  <child link="finger_right"/>
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<joint name="left_finger_joint" type="prismatic">
  <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
  <axis xyz="0 1 0"/>
  <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
  <parent link="base"/>
  <child link="finger_left"/>
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<ros2_control>
  <joint name="right_finger_joint">
    <command_interface name="effort"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>
  <joint name="left_finger_joint">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>
</ros2_control>

If one wants to disable mimic behaviour for some reason, they can still turn this functionality off by adding mimic="false" to the joint like so

  <joint name="left_finger_joint" mimic="false">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>

[destogl]

This is a new specification.

Mimic structure should be added to JointInfo. If mimic is true, then the joint may not have command interfaces.

<joint name="right_finger_joint" type="prismatic">
  <axis xyz="0 1 0"/>
  <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
  <parent link="base"/>
  <child link="finger_right"/>
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<joint name="left_finger_joint" type="prismatic">
  <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
  <axis xyz="0 1 0"/>
  <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
  <parent link="base"/>
  <child link="finger_left"/>
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<ros2_control>
  <joint name="right_finger_joint">
    <command_interface name="effort"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>
  <joint name="left_finger_joint" mimic="true">
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

If one wants to disable mimic behavior for some reason, they can still turn this functionality off by adding mimic="false" to the joint like so:

  <joint name="left_finger_joint" mimic="false">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>

[christophfroehlich]

I missed the updates on this thread in summer somehow. The specification looks good to me.
Just to be sure: As I understand the URDF specification, <mimic> means that the position is mimicked? So we have to implement a position-mimic independent of the command interface type of the mimicked joint?

[christophfroehlich]

hi @destogl. I'd like to work on that issue, and you offered help in the other issue where to start from..
As you suggested in the initial post in this issue, I'd start here?
https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/src/component_parser.cpp#L546

[DLu]

Is there an XML spec that you're working from?

[christophfroehlich]

Is there an XML spec that you're working from?

No, there is no overall valid XML spec I'm afraid. The https://github.com/ros-controls/roadmap repository hosts some design proposals which might be or might not be implemented (yet).

[jclinton830]

Has there been any progress with this?