Mimic joints independent of the combination of command interfaces

README.md

@@ -95,9 +95,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"/>
@@ -110,15 +111,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
@@ -245,7 +273,7 @@ The following example shows parallel gripper with mimic joint:
 
 
 ```bash
-ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py
+ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py
 ```
 
 Send example commands:
@@ -254,6 +282,16 @@ 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 command interface of the joint to be mimicked, run
+
+```bash
+ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_effort.launch.py
+```
+instead.
+
+
+
 #### 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

@@ -31,7 +31,9 @@ struct MimicJoint
 {
   std::size_t joint_index;
   std::size_t mimicked_joint_index;
+  gazebo_ros2_control::GazeboSystemInterface::ControlMethod control_method;
   double multiplier = 1.0;
+  double offset = 0.0;
 };
 
 class gazebo_ros2_control::GazeboSystemPrivate
@@ -89,7 +91,7 @@ class gazebo_ros2_control::GazeboSystemPrivate
   /// \brief handles to the FT sensors from within Gazebo
   std::vector<gazebo::sensors::ForceTorqueSensorPtr> sim_ft_sensors_;
 
-  /// \brief An array per FT sensor for 3D force and torquee
+  /// \brief An array per FT sensor for 3D force and torque
   std::vector<std::array<double, 6>> ft_sensor_data_;
 
   /// \brief state interfaces that will be exported to the Resource Manager
@@ -185,16 +187,34 @@ void GazeboSystem::registerJoints(
       mimic_joint.joint_index = j;
       mimic_joint.mimicked_joint_index = std::distance(
         hardware_info.joints.begin(), mimicked_joint_it);
+      for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
+        if (joint_info.command_interfaces[i].name == "position") {
+          mimic_joint.control_method |= POSITION;
+        }
+        if (joint_info.command_interfaces[i].name == "velocity") {
+          mimic_joint.control_method |= VELOCITY;
+        }
+        if (joint_info.command_interfaces[i].name == "effort") {
+          mimic_joint.control_method |= EFFORT;
+        }
+      }
       auto param_it = joint_info.parameters.find("multiplier");
       if (param_it != joint_info.parameters.end()) {
         mimic_joint.multiplier = std::stod(joint_info.parameters.at("multiplier"));
       } 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 mimicking joint '" << mimicked_joint <<
+          "' with multiplier: " << mimic_joint.multiplier <<
+          "' and offset: " << mimic_joint.offset);
       this->dataPtr->mimic_joints_.push_back(mimic_joint);
       suffix = "_mimic";
     }
@@ -512,7 +532,7 @@ GazeboSystem::perform_command_mode_switch(
   // mimic joint has the same control mode as mimicked joint
   for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
     this->dataPtr->joint_control_methods_[mimic_joint.joint_index] =
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index];
+      mimic_joint.control_method;
   }
   return hardware_interface::return_type::OK;
 }
@@ -569,25 +589,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 → gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py

@@ -33,7 +33,7 @@ def generate_launch_description():
                 [FindPackageShare(
                     'gazebo_ros2_control_demos'),
                     'urdf',
-                    'test_gripper_mimic_joint.xacro.urdf']
+                    'test_gripper_mimic_joint_effort.xacro.urdf']
             ),
         ]
     )

gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py

@@ -0,0 +1,88 @@
+# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name='xacro')]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare(
+                    'gazebo_ros2_control_demos'),
+                    'urdf',
+                    'test_gripper_mimic_joint_position.xacro.urdf']
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[robot_description]
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"]
+        ),
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'gripper'],
+                        output='screen')
+
+    load_joint_state_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_gripper_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'gripper_controller'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        RegisterEventHandler(
+          event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_controller],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_controller,
+                on_exit=[load_gripper_controller],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])