From 36f4f494dbe710082ed0e2e74d81ba895a819341 Mon Sep 17 00:00:00 2001
From: David ter Kuile <david.terkuile@pal-robotics.com>
Date: Tue, 5 Mar 2024 18:04:11 +0100
Subject: [PATCH] Add temporary fix for mimic-joint issue
 https://github.com/ros-controls/gazebo_ros2_control/issues/173

---
 .../urdf/robotiq.ros2_control.xacro           | 160 ++++++------------
 .../urdf/robotiq_85_gripper.urdf.xacro        |  30 ++++
 2 files changed, 83 insertions(+), 107 deletions(-)

diff --git a/pal_robotiq_description/urdf/robotiq.ros2_control.xacro b/pal_robotiq_description/urdf/robotiq.ros2_control.xacro
index 1f28797..29360db 100644
--- a/pal_robotiq_description/urdf/robotiq.ros2_control.xacro
+++ b/pal_robotiq_description/urdf/robotiq.ros2_control.xacro
@@ -15,7 +15,23 @@ See the License for the specific language governing permissions and
 limitations under the License.
  -->
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:macro name="ros2_control_mimic_joint" params="mimic_joint_name actuated_joint_name multiplier" >
+
+      <joint name="${mimic_joint_name}">
+        <param name="mimic">${actuated_joint_name}</param>
+        <param name="multiplier">${multiplier}</param>
+        <command_interface name="position" />
+
+        <state_interface name="position" />
+        <state_interface name="velocity" />
+        <state_interface name="effort" />
+      </joint>
+      
+    </xacro:macro>
+
   <xacro:macro name="ros2_control_robotiq_85_gripper" params="name reduction" >
+
     <joint name="${name}_finger_joint">
       <command_interface name="position" />
 
@@ -24,64 +40,30 @@ limitations under the License.
       <state_interface name="effort" />
     </joint>
 
-    <joint name="${name}_right_outer_knuckle_joint">
-      <param name="mimic">${name}_finger_joint</param>
-      <param name="multiplier">1</param>
-      <command_interface name="position"/>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_outer_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/> 
 
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    
-    <joint name="${name}_right_inner_knuckle_joint">
-      <param name="mimic">${name}_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>
-
-    <joint name="${name}_right_inner_finger_joint">
-      <param name="mimic">${name}_finger_joint</param>
-      <param name="multiplier">-1</param>
-      <command_interface name="position"/>
+   <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
 
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_finger_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="-1"/>
 
-    <joint name="${name}_left_inner_finger_joint">
-      <param name="mimic">${name}_finger_joint</param>
-      <param name="multiplier">-1</param>
-      <command_interface name="position"/>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_finger_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="-1"/>
 
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="${name}_left_inner_knuckle_joint">
-      <param name="mimic">${name}_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>
-    <joint name="${name}_left_outer_knuckle_joint">
-      <param name="mimic">${name}_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>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
 
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_outer_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
+    
     <xacro:include filename="$(find pal_robotiq_description)/urdf/robotiq.transmission.xacro" />
     <xacro:robotiq_transmission prefix="${name}_finger" reduction="${reduction}" />
 
@@ -96,70 +78,34 @@ limitations under the License.
       <state_interface name="effort" />
     </joint>
 
-    <joint name="${name}_right_outer_knuckle_joint">
-      <param name="mimic">${name}_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>
-
-    <joint name="${name}_right_inner_knuckle_joint">
-      <param name="mimic">${name}_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>
 
-    <joint name="${name}_right_inner_finger_joint">
-      <param name="mimic">${name}_finger_joint</param>
-      <param name="multiplier">1</param>
-      <command_interface name="position"/>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_outer_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
 
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
 
-    <joint name="${name}_left_inner_finger_joint">
-      <param name="mimic">${name}_finger_joint</param>
-      <param name="multiplier">1</param>
-      <command_interface name="position"/>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_finger_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="-1"/>
 
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_finger_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="-1"/>
 
-    <joint name="${name}_left_inner_knuckle_joint">
-      <param name="mimic">${name}_finger_joint</param>
-      <param name="multiplier">-1</param>
-      <command_interface name="position"/>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
 
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="${name}_left_outer_knuckle_joint">
-      <param name="mimic">${name}_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>
+    <xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_outer_knuckle_joint" 
+                                    actuated_joint_name="${name}_finger_joint" 
+                                    multiplier="1"/>
 
     <xacro:include filename="$(find pal_robotiq_description)/urdf/robotiq.transmission.xacro" />
     <xacro:robotiq_transmission prefix="${name}_finger" reduction="${reduction}" />
 
   </xacro:macro>
 
-
-
 </robot>
diff --git a/pal_robotiq_description/urdf/robotiq_85_gripper.urdf.xacro b/pal_robotiq_description/urdf/robotiq_85_gripper.urdf.xacro
index 87fa3b4..8e50947 100644
--- a/pal_robotiq_description/urdf/robotiq_85_gripper.urdf.xacro
+++ b/pal_robotiq_description/urdf/robotiq_85_gripper.urdf.xacro
@@ -333,6 +333,29 @@
     <xacro:inner_knuckle prefix="${prefix}" fingerprefix="${fingerprefix}" reflect="${reflect}"/>
   </xacro:macro>
 
+    <!--Dummy joint/link to prevent error with mimic joint
+    See https://github.com/ros-controls/gazebo_ros2_control/issues/173
+    -->
+
+ <xacro:macro name="dummy_mimic_joint" params="name parent">
+
+    <link name="${name}_dummy_mimic_link"/>
+
+    <joint name="${name}_joint_mimic" type="fixed">
+      <parent link="${parent}" />
+      <child link="${name}_dummy_mimic_link" />
+    </joint>
+
+  </xacro:macro>
+
+  <xacro:macro name="finger_mimic_dummy" params="prefix fingerprefix">
+
+    <xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_outer_finger" parent="${prefix}base_link"/>
+    <xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_inner_finger" parent="${prefix}base_link"/>
+    <xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_inner_finger_pad" parent="${prefix}base_link"/>
+    <xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_inner_knuckle" parent="${prefix}base_link"/>
+  </xacro:macro>
+
   <xacro:macro name="finger_joint" params="prefix">
     <joint name="${prefix}finger_joint" type="revolute">
       <origin xyz="0 -0.0306011 0.054904" rpy="0 0 ${pi}"/>
@@ -371,12 +394,19 @@
     <xacro:pal_robotiq_coupling_link prefix="${prefix}"/>
     <xacro:finger_links prefix="${prefix}" fingerprefix="left" reflect="1"/>
     <xacro:finger_links prefix="${prefix}" fingerprefix="right" reflect="-1"/>
+
+    <xacro:finger_mimic_dummy prefix="${prefix}" fingerprefix="left"/>
+    <xacro:finger_mimic_dummy prefix="${prefix}" fingerprefix="right"/>
+
     <xacro:robotiq_base_joint prefix="${prefix}" />
     <xacro:pal_robotiq_coupling_joint prefix="${prefix}" parent="${parent}">
       <xacro:insert_block name="origin" />
     </xacro:pal_robotiq_coupling_joint>
     <xacro:finger_joint prefix="${prefix}"/>
     <xacro:right_outer_knuckle_joint prefix="${prefix}"/>
+
+    <xacro:dummy_mimic_joint name="${prefix}right_outer_knuckle" parent="${prefix}base_link"/>
+
     <!--This xacro is used to be able to grasp object in simulmation more robustly, it could cause segfault at grasping time -->
     <xacro:gazebo_gripper_grasp name="${prefix}grasp" name_prefix_left="${prefix}left_inner_finger" name_prefix_right="${prefix}right_inner_finger" parent_joint="${parent}"/>
   </xacro:macro>