From 743569c1afdd34632fbb51738d19c39bee916071 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sun, 7 Nov 2021 15:32:35 +0100
Subject: [PATCH] Define meaningful tool frame
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

When a hand is mounted, the link8 frame is 45° rotated wrt. the hand.
Better define a tool frame that is nicely aligned with the hand, i.e.
x=normal, y=slide, z=approach, and nicely centered between the fingers.
---
 franka_description/robots/panda_arm.urdf.xacro | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index d5b8d0061..d567f7ea8 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -18,6 +18,21 @@
     </xacro:if>
   </xacro:unless>
 
+  <!-- Define a tool frame -->
+  <link name="$(arg arm_id)_tool" />
+  <joint name="$(arg arm_id)_tool_joint" type="fixed">
+    <xacro:if value="$(arg hand)">
+      <!-- Place between the fingers, orientation: x=normal, y=slide, z=approach -->
+      <origin xyz="0 0 0.1034" rpy="0 0 ${-pi/4}" />
+    </xacro:if>
+    <!-- Identical to link8 -->
+    <xacro:unless value="$(arg hand)">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:unless>
+    <parent link="$(arg arm_id)_link8" />
+    <child link="$(arg arm_id)_tool" />
+  </joint>
+
   <xacro:if value="$(arg gazebo)">
 
     <xacro:arg name="xyz" default="0 0 0" />