Rewrite mimic joints (#276)

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
ros-controls · Apr 23, 2024 · d3e00a5 · d3e00a5
1 parent f553c42
commit d3e00a5
Show file tree

Hide file tree

Showing 11 changed files with 343 additions and 155 deletions.
diff --git a/doc/img/gz_gripper.gif b/doc/img/gz_gripper.gif
diff --git a/doc/index.rst b/doc/index.rst
@@ -102,33 +102,27 @@ include:
 Using mimic joints in simulation
 -----------------------------------------------------------
 
-To use ``mimic`` joints in *gz_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
+To use ``mimic`` joints in *gz_ros2_control* you should define its parameters in your URDF, i.e, set the ``<mimic>`` tag to the mimicked joint (see the `URDF specification <https://wiki.ros.org/urdf/XML/joint>`__)
 
 .. code-block:: xml
 
+  <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"/>
+    <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>
 
-.. code-block:: 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>
+The mimic joint must not have command interfaces configured in the ``<ros2_control>`` tag, but state interfaces can be configured.
 
 
 Add the gz_ros2_control plugin
@@ -244,8 +238,19 @@ The following example shows parallel gripper with mimic joint:
 
 .. code-block:: shell
 
-  ros2 launch gz_ros2_control_demos gripper_mimic_joint_example.launch.py
+  ros2 launch gz_ros2_control_demos gripper_mimic_joint_example_position.launch.py
+
+.. image:: img/gz_gripper.gif
+  :alt: 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
+
+.. code-block:: shell
+
+  ros2 launch gz_ros2_control_demos gripper_mimic_joint_example_effort.launch.py
 
+instead.
 
 Send example commands:
 

diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp
@@ -100,14 +100,6 @@ struct jointData
   gz_ros2_control::GazeboSimSystemInterface::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:
@@ -175,9 +167,6 @@ class gz_ros2_control::GazeboSimSystemPrivate
   /// \brief Gazebo communication node.
   GZ_TRANSPORT_NAMESPACE Node node;
 
-  /// \brief mapping of mimicked joints to index of joint they mimic
-  std::vector<MimicJoint> mimic_joints_;
-
   /// \brief Gain which converts position error to a velocity command
   double position_proportional_gain_;
 
@@ -287,58 +276,20 @@ bool GazeboSimSystem::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 = hardware_interface::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");
-      }
+    auto it = std::find_if(
+      hardware_info.mimic_joints.begin(),
+      hardware_info.mimic_joints.end(),
+      [j](const hardware_interface::MimicJoint & mj) {
+        return mj.joint_index == j;
+      });
+
+    if (it != hardware_info.mimic_joints.end()) {
       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";
+        "Joint '" << joint_name << "'is mimicking joint '" <<
+          this->dataPtr->joints_[it->mimicked_joint_index].name <<
+          "' with multiplier: " << it->multiplier << " and offset: " << it->offset);
     }
 
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
@@ -373,7 +324,7 @@ bool GazeboSimSystem::initSim(
       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 + suffix,
+          joint_name,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joints_[j].joint_position);
         initial_position = get_initial_value(joint_info.state_interfaces[i]);
@@ -382,7 +333,7 @@ bool GazeboSimSystem::initSim(
       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 + suffix,
+          joint_name,
           hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joints_[j].joint_velocity);
         initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
@@ -391,7 +342,7 @@ bool GazeboSimSystem::initSim(
       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 + suffix,
+          joint_name,
           hardware_interface::HW_IF_EFFORT,
           &this->dataPtr->joints_[j].joint_effort);
         initial_effort = get_initial_value(joint_info.state_interfaces[i]);
@@ -406,7 +357,7 @@ bool GazeboSimSystem::initSim(
       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 + suffix,
+          joint_name,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joints_[j].joint_position_cmd);
         if (!std::isnan(initial_position)) {
@@ -415,7 +366,7 @@ bool GazeboSimSystem::initSim(
       } 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 + suffix,
+          joint_name,
           hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joints_[j].joint_velocity_cmd);
         if (!std::isnan(initial_velocity)) {
@@ -424,7 +375,7 @@ bool GazeboSimSystem::initSim(
       } else if (joint_info.command_interfaces[i].name == "effort") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name + suffix,
+          joint_name,
           hardware_interface::HW_IF_EFFORT,
           &this->dataPtr->joints_[j].joint_effort_cmd);
         if (!std::isnan(initial_effort)) {
@@ -732,76 +683,31 @@ hardware_interface::return_type GazeboSimSystem::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<sim::components::JointPosition>(
-          this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
+  for (const auto & mimic_joint : this->info_.mimic_joints) {
+    // Get the joint position
+    double position_mimicked_joint =
+      this->dataPtr->ecm->Component<sim::components::JointPosition>(
+      this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
 
-        double position_mimic_joint =
-          this->dataPtr->ecm->Component<sim::components::JointPosition>(
-          this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
+    double position_mimic_joint =
+      this->dataPtr->ecm->Component<sim::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 position_error =
+      position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;
 
-        double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
+    double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
 
-        auto vel =
-          this->dataPtr->ecm->Component<sim::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,
-            sim::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<sim::components::JointVelocity>(
-          this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
-
-        if (!this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
-        {
-          this->dataPtr->ecm->CreateComponent(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
-            sim::components::JointVelocityCmd({0}));
-        } else {
-          const auto jointVelCmd =
-            this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
-          *jointVelCmd = sim::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<sim::components::JointForce>(
-        //   this->dataPtr->sim_joints_[j]);
-        if (!this->dataPtr->ecm->Component<sim::components::JointForceCmd>(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
-        {
-          this->dataPtr->ecm->CreateComponent(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
-            sim::components::JointForceCmd({0}));
-        } else {
-          const auto jointEffortCmd =
-            this->dataPtr->ecm->Component<sim::components::JointForceCmd>(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
-          *jointEffortCmd = sim::components::JointForceCmd(
-            {mimic_joint.multiplier *
-              this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort});
-        }
-      }
+    auto vel =
+      this->dataPtr->ecm->Component<sim::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,
+        sim::components::JointVelocityCmd({velocity_sp}));
+    } else if (!vel->Data().empty()) {
+      vel->Data()[0] = velocity_sp;
     }
   }
 

diff --git a/gz_ros2_control_demos/config/gripper_controller_effort.yaml b/gz_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
diff --git a/...trol_demos/config/gripper_controller.yaml → ...s/config/gripper_controller_position.yaml b/...trol_demos/config/gripper_controller.yaml → ...s/config/gripper_controller_position.yaml
diff --git a/...nch/gripper_mimic_joint_example.launch.py → ...pper_mimic_joint_example_effort.launch.py b/...nch/gripper_mimic_joint_example.launch.py → ...pper_mimic_joint_example_effort.launch.py
@@ -37,7 +37,7 @@ def generate_launch_description():
             " ",
             PathJoinSubstitution(
                 [FindPackageShare("gz_ros2_control_demos"),
-                 "urdf", "test_gripper_mimic_joint.xacro.urdf"]
+                 "urdf", "test_gripper_mimic_joint_effort.xacro.urdf"]
             ),
         ]
     )

diff --git a/...ch/gripper_mimic_joint_example.launch.xml → ...per_mimic_joint_example_effort.launch.xml b/...ch/gripper_mimic_joint_example.launch.xml → ...per_mimic_joint_example_effort.launch.xml
@@ -25,7 +25,7 @@ Author: Dr. Denis
        description="Start RViz2 automatically with this launch file."/>
 
   <!-- Load description and start controllers -->
-  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share gz_ros2_control_demos)/urdf/test_gripper_mimic_joint.xacro.urdf')" />
+  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share gz_ros2_control_demos)/urdf/test_gripper_mimic_joint_effort.xacro.urdf')" />
 
   <node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
     <param name="robot_description" value="$(var robot_description_content)" />