From 046363691136981a7b42fad1c3f13331f17c8a29 Mon Sep 17 00:00:00 2001
From: Fabian D <fabian.dubois@gmail.com>
Date: Tue, 15 Aug 2023 16:39:50 +0900
Subject: [PATCH 1/2] Fix gripper mimic joint control

---
 urdf/crane_x7.gazebo_ros2_control.xacro | 11 ++++++++++-
 urdf/crane_x7.ros2_control.xacro        |  2 +-
 urdf/crane_x7.urdf.xacro                |  1 +
 3 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/urdf/crane_x7.gazebo_ros2_control.xacro b/urdf/crane_x7.gazebo_ros2_control.xacro
index 66f4208..1b96a00 100644
--- a/urdf/crane_x7.gazebo_ros2_control.xacro
+++ b/urdf/crane_x7.gazebo_ros2_control.xacro
@@ -11,6 +11,7 @@
             name_joint_6
             name_joint_7
             name_joint_gripper_finger_a
+            name_joint_gripper_finger_b
             joint_1_lower_limit
             joint_1_upper_limit
             joint_2_lower_limit
@@ -113,7 +114,15 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+    <joint name="${name_joint_gripper_finger_b}">
+      <param name="mimic">${name_joint_gripper_finger_a}</param>
+      <param name="multiplier">1</param>
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
     </ros2_control>
   </xacro:macro>
-</robot>
\ No newline at end of file
+</robot>
diff --git a/urdf/crane_x7.ros2_control.xacro b/urdf/crane_x7.ros2_control.xacro
index d6af75d..68a63b7 100644
--- a/urdf/crane_x7.ros2_control.xacro
+++ b/urdf/crane_x7.ros2_control.xacro
@@ -134,4 +134,4 @@
 
     </ros2_control>
   </xacro:macro>
-</robot>
\ No newline at end of file
+</robot>
diff --git a/urdf/crane_x7.urdf.xacro b/urdf/crane_x7.urdf.xacro
index 7a791c5..e86cbd1 100644
--- a/urdf/crane_x7.urdf.xacro
+++ b/urdf/crane_x7.urdf.xacro
@@ -256,6 +256,7 @@
       name_joint_6="${NAME_JOINT_6}"
       name_joint_7="${NAME_JOINT_7}"
       name_joint_gripper_finger_a="${NAME_JOINT_GRIPPER_FINGER_A}"
+      name_joint_gripper_finger_b="${NAME_JOINT_GRIPPER_FINGER_B}"
       joint_1_lower_limit="${JOINT_1_LOWER_LIMIT}"
       joint_1_upper_limit="${JOINT_1_UPPER_LIMIT}"
       joint_2_lower_limit="${JOINT_2_LOWER_LIMIT}"

From 00c7a6a9aa7b4d597e988f45a165782a789999a7 Mon Sep 17 00:00:00 2001
From: Fabian D <fabian.dubois@gmail.com>
Date: Tue, 15 Aug 2023 21:32:23 +0900
Subject: [PATCH 2/2] Add dummy joint to prevent Joint not found error

[move_group-3] [ERROR] [1692102600.709104192] [moveit_robot_model.robot_model]: Joint 'crane_x7_gripper_finger_b_joint_mimic' not found in model 'crane_x7'

See https://github.com/ros-controls/gazebo_ros2_control/issues/173
---
 urdf/crane_x7_wide_two_finger_gripper.xacro | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/urdf/crane_x7_wide_two_finger_gripper.xacro b/urdf/crane_x7_wide_two_finger_gripper.xacro
index 364e8d9..5dc2ebf 100644
--- a/urdf/crane_x7_wide_two_finger_gripper.xacro
+++ b/urdf/crane_x7_wide_two_finger_gripper.xacro
@@ -78,6 +78,15 @@
       </inertial>
     </link>
 
+    <!--Dummy joint/link to prevent error with mimic joint
+    See https://github.com/ros-controls/gazebo_ros2_control/issues/173
+    -->
+    <joint name="${name_joint_gripper_finger_b}_mimic" type="fixed">
+      <parent link="world" />
+      <child link="dummy_mimic_fix" />
+    </joint>
+    <link name="dummy_mimic_fix"/>
+
     <joint name="${name_joint_gripper_finger_b}" type="revolute">
       <origin xyz="0.012 0.0 0.024" rpy="0 0 0"/>
       <axis xyz="0 1 0"/>