Support for mimic joints and example with gripper.

README.md

@@ -88,6 +88,37 @@ include
 </ros2_control>
 ```
 
+### Using mimic joints in simulation
+
+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
+
+```xml
+<joint name="left_finger_joint" type="prismatic">
+  <mimic joint="right_finger_joint"/>
+  <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>
+```
+
+```xml
+<joint name="left_finger_joint">
+  <param name="mimic">right_finger_joint</param>
+  <param name="multiplier">1</param>
+  <command_interface name="position"/>
+  <state_interface name="position"/>
+  <state_interface name="velocity"/>
+  <state_interface name="effort"/>
+</joint>
+```
+
+
 ## Add the gazebo_ros2_control plugin
 
 In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that
@@ -202,6 +233,21 @@ ros2 run gazebo_ros2_control_demos example_velocity
 ros2 run gazebo_ros2_control_demos example_effort
 ```
 
+The following example shows parallel gripper with mimic joint:
+
+![](img/gripper.gif)
+
+
+```bash
+ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py
+```
+
+Send example commands:
+
+```bash
+ros2 run gazebo_ros2_control_demos example_gripper
+```
+
 #### 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

@@ -27,6 +27,13 @@
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 
+struct MimicJoint
+{
+  std::size_t joint_index;
+  std::size_t mimicked_joint_index;
+  double multiplier = 1.0;
+};
+
 class gazebo_ros2_control::GazeboSystemPrivate
 {
 public:
@@ -90,6 +97,9 @@ class gazebo_ros2_control::GazeboSystemPrivate
 
   /// \brief command interfaces that will be exported to the Resource Manager
   std::vector<hardware_interface::CommandInterface> command_interfaces_;
+
+  /// \brief mapping of mimicked joints to index of joint they mimic
+  std::vector<MimicJoint> mimic_joints_;
 };
 
 namespace gazebo_ros2_control
@@ -142,7 +152,8 @@ void GazeboSystem::registerJoints(
   this->dataPtr->joint_effort_cmd_.resize(this->dataPtr->n_dof_);
 
   for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
-    std::string joint_name = this->dataPtr->joint_names_[j] = hardware_info.joints[j].name;
+    auto & joint_info = hardware_info.joints[j];
+    std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
 
     gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
     this->dataPtr->sim_joints_.push_back(simjoint);
@@ -156,6 +167,38 @@ void GazeboSystem::registerJoints(
     // Accept this joint and continue configuration
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
 
+    std::string suffix = "";
+
+    // check if joint is mimicked
+    if (joint_info.parameters.find("mimic") != joint_info.parameters.end()) {
+      const auto mimicked_joint = joint_info.parameters.at("mimic");
+      const auto mimicked_joint_it = std::find_if(
+        hardware_info.joints.begin(), hardware_info.joints.end(),
+        [&mimicked_joint](const hardware_interface::ComponentInfo & info) {
+          return info.name == mimicked_joint;
+        });
+      if (mimicked_joint_it == hardware_info.joints.end()) {
+        throw std::runtime_error(
+                std::string("Mimicked joint '") + mimicked_joint + "' not found");
+      }
+      MimicJoint mimic_joint;
+      mimic_joint.joint_index = j;
+      mimic_joint.mimicked_joint_index = std::distance(
+        hardware_info.joints.begin(), mimicked_joint_it);
+      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;
+      }
+      RCLCPP_INFO_STREAM(
+        this->nh_->get_logger(),
+        "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << "' with mutiplier: "
+                  << mimic_joint.multiplier);
+      this->dataPtr->mimic_joints_.push_back(mimic_joint);
+      suffix = "_mimic";
+    }
+
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
 
     auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) {
@@ -173,67 +216,67 @@ void GazeboSystem::registerJoints(
     double initial_effort = std::numeric_limits<double>::quiet_NaN();
 
     // register the state handles
-    for (unsigned int i = 0; i < hardware_info.joints[j].state_interfaces.size(); i++) {
-      if (hardware_info.joints[j].state_interfaces[i].name == "position") {
+    for (unsigned int i = 0; i < joint_info.state_interfaces.size(); i++) {
+      if (joint_info.state_interfaces[i].name == "position") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
         this->dataPtr->state_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joint_position_[j]);
-        initial_position = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
+        initial_position = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joint_position_[j] = initial_position;
       }
-      if (hardware_info.joints[j].state_interfaces[i].name == "velocity") {
+      if (joint_info.state_interfaces[i].name == "velocity") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
         this->dataPtr->state_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joint_velocity_[j]);
-        initial_velocity = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
+        initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joint_velocity_[j] = initial_velocity;
       }
-      if (hardware_info.joints[j].state_interfaces[i].name == "effort") {
+      if (joint_info.state_interfaces[i].name == "effort") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
         this->dataPtr->state_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_EFFORT,
           &this->dataPtr->joint_effort_[j]);
-        initial_effort = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
+        initial_effort = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joint_effort_[j] = initial_effort;
       }
     }
 
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:");
 
     // register the command handles
-    for (unsigned int i = 0; i < hardware_info.joints[j].command_interfaces.size(); i++) {
-      if (hardware_info.joints[j].command_interfaces[i].name == "position") {
+    for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
+      if (joint_info.command_interfaces[i].name == "position") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
         this->dataPtr->joint_control_methods_[j] |= POSITION;
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joint_position_cmd_[j]);
         if (!std::isnan(initial_position)) {
           this->dataPtr->joint_position_cmd_[j] = initial_position;
         }
       }
-      if (hardware_info.joints[j].command_interfaces[i].name == "velocity") {
+      if (joint_info.command_interfaces[i].name == "velocity") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
         this->dataPtr->joint_control_methods_[j] |= VELOCITY;
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joint_velocity_cmd_[j]);
         if (!std::isnan(initial_velocity)) {
           this->dataPtr->joint_velocity_cmd_[j] = initial_velocity;
         }
       }
-      if (hardware_info.joints[j].command_interfaces[i].name == "effort") {
+      if (joint_info.command_interfaces[i].name == "effort") {
         this->dataPtr->joint_control_methods_[j] |= EFFORT;
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_EFFORT,
           &this->dataPtr->joint_effort_cmd_[j]);
         if (!std::isnan(initial_effort)) {
@@ -381,9 +424,9 @@ void GazeboSystem::registerSensors(
 }
 
 CallbackReturn
-GazeboSystem::on_init(const hardware_interface::HardwareInfo & actuator_info)
+GazeboSystem::on_init(const hardware_interface::HardwareInfo & system_info)
 {
-  if (hardware_interface::SystemInterface::on_init(actuator_info) != CallbackReturn::SUCCESS) {
+  if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) {
     return CallbackReturn::ERROR;
   }
   return CallbackReturn::SUCCESS;
@@ -457,6 +500,30 @@ hardware_interface::return_type GazeboSystem::write()
   rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
   rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
 
+  // 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] & 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] & EFFORT &&
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY)
+    {
+      this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
+        mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
+    }
+  }
+
   for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
     if (this->dataPtr->sim_joints_[j]) {
       if (this->dataPtr->joint_control_methods_[j] & POSITION) {
@@ -470,8 +537,7 @@ hardware_interface::return_type GazeboSystem::write()
           this->dataPtr->joint_velocity_cmd_[j]);
       }
       if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
-        const double effort =
-          this->dataPtr->joint_effort_cmd_[j];
+        const double effort = this->dataPtr->joint_effort_cmd_[j];
         this->dataPtr->sim_joints_[j]->SetForce(0, effort);
       }
     }

gazebo_ros2_control_demos/CMakeLists.txt

@@ -46,6 +46,12 @@ ament_target_dependencies(example_effort
   std_msgs
 )
 
+add_executable(example_gripper examples/example_gripper)
+ament_target_dependencies(example_gripper
+  rclcpp
+  std_msgs
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
@@ -54,7 +60,7 @@ if(BUILD_TESTING)
 endif()
 
 ## Install
-install(TARGETS example_position example_velocity example_effort
+install(TARGETS example_position example_velocity example_effort example_gripper
   DESTINATION lib/${PROJECT_NAME}
 )
 

gazebo_ros2_control_demos/config/gripper_controller.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: position

gazebo_ros2_control_demos/examples/example_gripper.cpp

@@ -0,0 +1,58 @@
+// 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.
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/float64_multi_array.hpp"
+
+std::shared_ptr<rclcpp::Node> node;
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  node = std::make_shared<rclcpp::Node>("gripper_test_node");
+
+  auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
+    "/gripper_controller/commands", 10);
+
+  RCLCPP_INFO(node->get_logger(), "node created");
+
+  std_msgs::msg::Float64MultiArray commands;
+
+  using namespace std::chrono_literals;
+
+  commands.data.push_back(0);
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 0.38;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 0.19;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 0;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+  rclcpp::shutdown();
+
+  return 0;
+}