From 9a32d57cfcf94795cc488bc294d608e7f2cb522f Mon Sep 17 00:00:00 2001
From: Krzysztof Wojciechowski <krzy.wojciecho@gmail.com>
Date: Wed, 27 Nov 2024 10:23:17 +0000
Subject: [PATCH] Add option to lock finger joints

---
 end_effectors/common/franka_hand.xacro               | 12 +++++++++---
 end_effectors/franka_hand/franka_hand.urdf.xacro     |  3 ++-
 .../franka_hand/franka_hand_arguments.xacro          |  3 +++
 robots/common/franka_robot.xacro                     |  1 +
 4 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/end_effectors/common/franka_hand.xacro b/end_effectors/common/franka_hand.xacro
index f2a5529..f2732e8 100644
--- a/end_effectors/common/franka_hand.xacro
+++ b/end_effectors/common/franka_hand.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
   <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
-  <xacro:macro name="franka_hand" params="connected_to:='' arm_id ee_id ee_inertials rpy:='0 0 0' xyz:='0 0 0' tcp_xyz:='0 0 0.1034' tcp_rpy:='0 0 0' safety_distance:=0 gazebo:=false description_pkg:=franka_description with_sc:=false">
+  <xacro:macro name="franka_hand" params="connected_to:='' arm_id ee_id ee_inertials rpy:='0 0 0' xyz:='0 0 0' tcp_xyz:='0 0 0.1034' tcp_rpy:='0 0 0' safety_distance:=0 gazebo:=false description_pkg:=franka_description with_sc:=false lock_finger_joints:=false">
     <xacro:unless value="${connected_to == ''}">
       <joint name="${arm_id}_hand_joint" type="fixed">
         <parent link="${connected_to}" />
@@ -101,7 +101,13 @@
         <xacro:ee-inertials name="rightfinger" />
       </xacro:if>
     </link>
-    <joint name="${arm_id}_finger_joint1" type="prismatic">
+    <xacro:if value="${lock_finger_joints}">
+      <xacro:property name="finger_joint_type" value="fixed" />
+    </xacro:if>
+    <xacro:unless value="${lock_finger_joints}">
+      <xacro:property name="finger_joint_type" value="prismatic" />
+    </xacro:unless>
+    <joint name="${arm_id}_finger_joint1" type="${finger_joint_type}">
       <parent link="${arm_id}_hand" />
       <child link="${arm_id}_leftfinger" />
       <origin xyz="0 0 0.0584" rpy="0 0 0" />
@@ -109,7 +115,7 @@
       <limit effort="100" lower="0.0" upper="0.04" velocity="0.2" />
       <dynamics damping="0.3" />
     </joint>
-    <joint name="${arm_id}_finger_joint2" type="prismatic">
+    <joint name="${arm_id}_finger_joint2" type="${finger_joint_type}">
       <parent link="${arm_id}_hand" />
       <child link="${arm_id}_rightfinger" />
       <origin xyz="0 0 0.0584" rpy="0 0 0" />
diff --git a/end_effectors/franka_hand/franka_hand.urdf.xacro b/end_effectors/franka_hand/franka_hand.urdf.xacro
index b74b47b..4a48bdd 100644
--- a/end_effectors/franka_hand/franka_hand.urdf.xacro
+++ b/end_effectors/franka_hand/franka_hand.urdf.xacro
@@ -15,7 +15,8 @@
                      safety_distance="$(arg safety_distance)"
                      gazebo="$(arg gazebo)"
                      description_pkg="$(arg description_pkg)"
-                     with_sc="$(arg with_sc)">
+                     with_sc="$(arg with_sc)"
+                     lock_finger_joints="$(arg lock_finger_joints)">
    </xacro:franka_hand>
 
 </robot>
diff --git a/end_effectors/franka_hand/franka_hand_arguments.xacro b/end_effectors/franka_hand/franka_hand_arguments.xacro
index 8129c95..cb90b9d 100644
--- a/end_effectors/franka_hand/franka_hand_arguments.xacro
+++ b/end_effectors/franka_hand/franka_hand_arguments.xacro
@@ -37,4 +37,7 @@
   <!-- Should fake sensors be used? -->
   <xacro:arg name="description_pkg" default="franka_description" />
 
+  <!-- Should finger joints be locked? -->
+  <xacro:arg name="lock_finger_joints" default="false" />
+
 </robot>
\ No newline at end of file
diff --git a/robots/common/franka_robot.xacro b/robots/common/franka_robot.xacro
index f595c85..50dc297 100644
--- a/robots/common/franka_robot.xacro
+++ b/robots/common/franka_robot.xacro
@@ -58,6 +58,7 @@
         gazebo="${gazebo}"
         description_pkg="$(arg description_pkg)"
         with_sc="${with_sc}"
+        lock_finger_joints="$(arg lock_finger_joints)"
       />
     </xacro:if>