Mimic joints independent of the combination of command interfaces

README.md

@@ -93,9 +93,10 @@ include
 To use `mimic` joints in `gazebo_ros2_control` you should define its parameters to your URDF.
 We should include:
 
-- `<mimic>` tag to the mimicked joint ([detailed manual(https://wiki.ros.org/urdf/XML/joint))
-- `mimic` and `multiplier` parameters to joint definition in `<ros2_control>` tag
+- `<mimic>` tag to the mimicked joint ([detailed manual](https://wiki.ros.org/urdf/XML/joint))
+- `mimic`, and optional `multiplier`+`offset` parameters to joint definition in `<ros2_control>` tag
 
+As an example, `left_finger_joint` mimics the position of `right_finger_joint`
 ```xml
 <joint name="left_finger_joint" type="prismatic">
   <mimic joint="right_finger_joint"/>
@@ -108,15 +109,42 @@ We should include:
 ```
 
 ```xml
+<joint name="right_finger_joint">
+  <command_interface name="position"/>
+  <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>
 ```
+You can specify a mimicked joint independent of the combination of command interfaces. E.g., if you use an effort command interface for joint 1 but want to let joint 2 mimic the position of joint 1, set
+```xml
+<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>
+```
+Be aware that these mimicked joints do not preserve any conservation of energy, i.e.,
+the necessary effort of joint 1 won't be changed.
 
 
 ## Add the gazebo_ros2_control plugin
@@ -248,6 +276,15 @@ Send example commands:
 ros2 run gazebo_ros2_control_demos example_gripper
 ```
 
+To demonstrate the setup of the initial position and a position-mimicked joint in
+case of an effort interface on joint 1, add the launch parameter
+
+```bash
+ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py use_effort:=true
+```
+
+
+
 #### Gazebo + Moveit2 + ROS 2
 
 This example works with [ROS 2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/).

gazebo_ros2_control/src/gazebo_system.cpp

@@ -32,6 +32,7 @@ struct MimicJoint
   std::size_t joint_index;
   std::size_t mimicked_joint_index;
   double multiplier = 1.0;
+  double offset = 0.0;
 };
 
 class gazebo_ros2_control::GazeboSystemPrivate
@@ -191,10 +192,17 @@ void GazeboSystem::registerJoints(
       } else {
         mimic_joint.multiplier = 1.0;
       }
+      param_it = joint_info.parameters.find("offset");
+      if (param_it != joint_info.parameters.end()) {
+        mimic_joint.offset = std::stod(joint_info.parameters.at("offset"));
+      } else {
+        mimic_joint.offset = 0.0;
+      }
       RCLCPP_INFO_STREAM(
         this->nh_->get_logger(),
-        "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << "' with mutiplier: "
-                  << mimic_joint.multiplier);
+        "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint <<
+          "' with mutiplier: " << mimic_joint.multiplier <<
+          "' and offset: " << mimic_joint.offset);
       this->dataPtr->mimic_joints_.push_back(mimic_joint);
       suffix = "_mimic";
     }
@@ -502,25 +510,46 @@ hardware_interface::return_type GazeboSystem::write()
 
   // set values of all mimic joints with respect to mimicked joint
   for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
-    if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION &&
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION)
-    {
-      this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
-        mimic_joint.multiplier *
-        this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
+    if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION) {
+      if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) {
+        // mimic position with identical joint_position_cmd
+        this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
+          mimic_joint.offset + mimic_joint.multiplier *
+          this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
+      } else {
+        // mimic position, if mimicked joint is velocity- or effort-controlled
+        this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
+          mimic_joint.offset + mimic_joint.multiplier *
+          this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index];
+      }
     }
-    if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY &&
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY)
-    {
-      this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
-        mimic_joint.multiplier *
-        this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
+
+    if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY) {
+      if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) {
+        // mimic velocity with identical joint_velocity_cmd_
+        this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
+          mimic_joint.offset + mimic_joint.multiplier *
+          this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
+      } else {
+        // mimic velocity, if mimicked joint is position- or effort-controlled
+        this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
+          mimic_joint.offset + mimic_joint.multiplier *
+          this->dataPtr->joint_velocity_[mimic_joint.mimicked_joint_index];
+      }
     }
-    if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT &&
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT)
-    {
-      this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
-        mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
+
+    if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT) {
+      if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) {
+        // mimic effort with identical joint_effort_cmd_
+        this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
+          mimic_joint.offset + mimic_joint.multiplier *
+          this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
+      } else {
+        // mimic effort, if mimicked joint is velocity- or position-controlled
+        this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
+          mimic_joint.offset + mimic_joint.multiplier *
+          this->dataPtr->joint_effort_[mimic_joint.mimicked_joint_index];
+      }
     }
   }
 

gazebo_ros2_control_demos/config/gripper_controller_effort.yaml

@@ -0,0 +1,15 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    gripper_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+gripper_controller:
+  ros__parameters:
+    joints:
+      - right_finger_joint
+    interface_name: effort

gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py

@@ -15,36 +15,44 @@
 # Author: Denis Stogl
 
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.actions import RegisterEventHandler, OpaqueFunction, DeclareLaunchArgument
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+import xacro
+import os
+from ament_index_python.packages import get_package_share_directory
 
 
-def generate_launch_description():
+# evaluates LaunchConfigurations in context for use with
+# xacro.process_file(). Returns a list of launch actions to be included in
+# launch description
+def evaluate_xacro(context, *args, **kwargs):
 
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name='xacro')]),
-            " ",
-            PathJoinSubstitution(
-                [FindPackageShare(
-                    'gazebo_ros2_control_demos'),
-                    'urdf',
-                    'test_gripper_mimic_joint.xacro.urdf']
-            ),
-        ]
-    )
-    robot_description = {"robot_description": robot_description_content}
+    use_effort = LaunchConfiguration('use_effort').perform(context)
+    print(use_effort)
+    xacro_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'),
+        'urdf',
+        'test_gripper_mimic_joint.xacro.urdf')
 
-    node_robot_state_publisher = Node(
+    robot_state_publisher_node = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
+        name='robot_state_publisher',
         output='screen',
-        parameters=[robot_description]
-    )
+        parameters=[{
+            'robot_description':
+                xacro.process_file(xacro_path,
+                                   mappings={'effort': use_effort}).toxml()
+        }])
+    return [robot_state_publisher_node]
+
+
+def generate_launch_description():
 
     gazebo = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -70,8 +78,13 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_effort',
+            default_value='False',
+            description='Use effort instead of position command interface'),
+
         RegisterEventHandler(
-          event_handler=OnProcessExit(
+            event_handler=OnProcessExit(
                 target_action=spawn_entity,
                 on_exit=[load_joint_state_controller],
             )
@@ -83,6 +96,8 @@ def generate_launch_description():
             )
         ),
         gazebo,
-        node_robot_state_publisher,
+        # add OpaqueFunction to evaluate xacro file in context and pass to any
+        # nodes that need it
+        OpaqueFunction(function=evaluate_xacro),
         spawn_entity,
     ])

gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf

@@ -1,5 +1,5 @@
 <?xml version="1.0" ?>
-<robot name="gripper">
+<robot name="gripper" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <link name="world"/>
   <link name="base">
     <visual>
@@ -71,7 +71,12 @@
       <plugin>gazebo_ros2_control/GazeboSystem</plugin>
     </hardware>
     <joint name="right_finger_joint">
-      <command_interface name="position"/>
+      <xacro:if value="$(arg effort)">
+        <command_interface name="effort"/>
+      </xacro:if>
+      <xacro:unless value="$(arg effort)">
+        <command_interface name="position"/>
+      </xacro:unless>
       <state_interface name="position">
         <param name="initial_value">0.15</param>
       </state_interface>
@@ -81,6 +86,7 @@
     <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"/>
@@ -90,7 +96,12 @@
 
   <gazebo>
     <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-      <parameters>$(find gazebo_ros2_control_demos)/config/gripper_controller.yaml</parameters>
+      <xacro:if value="$(arg effort)">
+        <parameters>$(find gazebo_ros2_control_demos)/config/gripper_controller_effort.yaml</parameters>
+      </xacro:if>
+      <xacro:unless value="$(arg effort)">
+        <parameters>$(find gazebo_ros2_control_demos)/config/gripper_controller_position.yaml</parameters>
+      </xacro:unless>
     </plugin>
   </gazebo>
 </robot>