Add support for mimic joints. (#33)

Signed-off-by: Denis Štogl <[email protected]>
Co-authored-by: Lovro <[email protected]>

README.md

@@ -118,6 +118,38 @@ include:
 </ros2_control>
 ```
 
+
+### Using mimic joints in simulation
+
+To use `mimic` joints in `ign_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 ign_ros2_control plugin
 
 In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that
@@ -233,3 +265,18 @@ ros2 run ign_ros2_control_demos example_effort
 ros2 run ign_ros2_control_demos example_diff_drive
 ros2 run ign_ros2_control_demos example_tricycle_drive
 ```
+
+The following example shows parallel gripper with mimic joint:
+
+![](img/gripper.gif)
+
+
+```bash
+ros2 launch ign_ros2_control_demos gripper_mimic_joint_example.launch.py
+```
+
+Send example commands:
+
+```bash
+ros2 run ign_ros2_control_demos example_gripper
+```

ign_ros2_control/src/ign_system.cpp

@@ -28,7 +28,6 @@
 #include <ignition/gazebo/components/JointForce.hh>
 #include <ignition/gazebo/components/JointForceCmd.hh>
 #include <ignition/gazebo/components/JointPosition.hh>
-#include <ignition/gazebo/components/JointPositionReset.hh>
 #include <ignition/gazebo/components/JointVelocity.hh>
 #include <ignition/gazebo/components/JointVelocityCmd.hh>
 #include <ignition/gazebo/components/LinearAcceleration.hh>
@@ -72,6 +71,14 @@ struct jointData
   ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method;
 };
 
+struct MimicJoint
+{
+  std::size_t joint_index;
+  std::size_t mimicked_joint_index;
+  double multiplier = 1.0;
+  std::vector<std::string> interfaces_to_mimic;
+};
+
 class ImuData
 {
 public:
@@ -138,6 +145,9 @@ class ign_ros2_control::IgnitionSystemPrivate
 
   /// \brief Ignition communication node.
   ignition::transport::Node node;
+
+  /// \brief mapping of mimicked joints to index of joint they mimic
+  std::vector<MimicJoint> mimic_joints_;
 };
 
 namespace ign_ros2_control
@@ -169,7 +179,8 @@ bool IgnitionSystem::initSim(
   }
 
   for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
-    std::string joint_name = this->dataPtr->joints_[j].name = hardware_info.joints[j].name;
+    auto & joint_info = hardware_info.joints[j];
+    std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name;
 
     ignition::gazebo::Entity simjoint = enableJoints[joint_name];
     this->dataPtr->joints_[j].sim_joint = simjoint;
@@ -201,6 +212,60 @@ bool IgnitionSystem::initSim(
     // 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;
+      }
+
+      // check joint info of mimicked joint
+      auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index];
+      const auto state_mimicked_interface = std::find_if(
+        joint_info_mimicked.state_interfaces.begin(), joint_info_mimicked.state_interfaces.end(),
+        [&mimic_joint](const hardware_interface::InterfaceInfo & interface_info) {
+          bool pos = interface_info.name == "position";
+          if (pos) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION);}
+          bool vel = interface_info.name == "velocity";
+          if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY);}
+          bool eff = interface_info.name == "effort";
+          if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT);}
+          return pos || vel || eff;
+        });
+      if (state_mimicked_interface == joint_info_mimicked.state_interfaces.end()) {
+        throw std::runtime_error(
+                std::string(
+                  "For mimic joint '") + joint_info.name +
+                "' no state interface was found in mimicked joint '" + mimicked_joint +
+                " ' to mimic");
+      }
+      RCLCPP_INFO_STREAM(
+        this->nh_->get_logger(),
+        "Joint '" << joint_name << "'is mimicking 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) {
@@ -218,62 +283,63 @@ bool IgnitionSystem::initSim(
     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->joints_[j].joint_position);
-        initial_position = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
+        initial_position = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joints_[j].joint_position = 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->joints_[j].joint_velocity);
-        initial_velocity = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
+        initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joints_[j].joint_velocity = 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->joints_[j].joint_effort);
-        initial_effort = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
+        initial_effort = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joints_[j].joint_effort = 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->command_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joints_[j].joint_position_cmd);
         if (!std::isnan(initial_position)) {
           this->dataPtr->joints_[j].joint_position_cmd = initial_position;
         }
-      } else if (hardware_info.joints[j].command_interfaces[i].name == "velocity") {
+      } else if (joint_info.command_interfaces[i].name == "velocity") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name,
+          joint_name + suffix,
           hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joints_[j].joint_velocity_cmd);
         if (!std::isnan(initial_velocity)) {
           this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity;
         }
-      } else if (hardware_info.joints[j].command_interfaces[i].name == "effort") {
+      } else if (joint_info.command_interfaces[i].name == "effort") {
+        this->dataPtr->joints_[j].joint_control_method |= 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->joints_[j].joint_effort_cmd);
         if (!std::isnan(initial_effort)) {
@@ -365,10 +431,10 @@ void IgnitionSystem::registerSensors(
 }
 
 CallbackReturn
-IgnitionSystem::on_init(const hardware_interface::HardwareInfo & actuator_info)
+IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info)
 {
   RCLCPP_WARN(this->nh_->get_logger(), "On init...");
-  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;
@@ -563,6 +629,80 @@ hardware_interface::return_type IgnitionSystem::write(
     }
   }
 
+  // set values of all mimic joints with respect to mimicked joint
+  for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
+    for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) {
+      if (mimic_interface == "position") {
+        // Get the joint position
+        double position_mimicked_joint =
+          this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
+          this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
+
+        double position_mimic_joint =
+          this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
+          this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
+
+        double position_error =
+          position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;
+
+        double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
+
+        auto vel =
+          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+          this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
+
+        if (vel == nullptr) {
+          this->dataPtr->ecm->CreateComponent(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
+            ignition::gazebo::components::JointVelocityCmd({velocity_sp}));
+        } else if (!vel->Data().empty()) {
+          vel->Data()[0] = velocity_sp;
+        }
+      }
+      if (mimic_interface == "velocity") {
+        // get the velocity of mimicked joint
+        double velocity_mimicked_joint =
+          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
+          this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
+
+        if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
+        {
+          this->dataPtr->ecm->CreateComponent(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
+            ignition::gazebo::components::JointVelocityCmd({0}));
+        } else {
+          const auto jointVelCmd =
+            this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
+          *jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
+            {mimic_joint.multiplier * velocity_mimicked_joint});
+        }
+      }
+      if (mimic_interface == "effort") {
+        // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124
+        // Get the joint force
+        // const auto * jointForce =
+        //   _ecm.Component<ignition::gazebo::components::JointForce>(
+        //   this->dataPtr->sim_joints_[j]);
+        if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
+        {
+          this->dataPtr->ecm->CreateComponent(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
+            ignition::gazebo::components::JointForceCmd({0}));
+        } else {
+          const auto jointEffortCmd =
+            this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
+          *jointEffortCmd = ignition::gazebo::components::JointForceCmd(
+            {mimic_joint.multiplier *
+              this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort});
+        }
+      }
+    }
+  }
+
   return hardware_interface::return_type::OK;
 }
 }  // namespace ign_ros2_control

ign_ros2_control_demos/CMakeLists.txt

@@ -59,6 +59,12 @@ ament_target_dependencies(example_tricycle_drive
   geometry_msgs
 )
 
+add_executable(example_gripper examples/example_gripper.cpp)
+ament_target_dependencies(example_gripper
+  rclcpp
+  std_msgs
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
@@ -74,6 +80,7 @@ install(
     example_effort
     example_diff_drive
     example_tricycle_drive
+    example_gripper
   DESTINATION
     lib/${PROJECT_NAME}
 )

ign_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