From f0ccb3982f94dd10276249ec955816900efa5c2c Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 12 Nov 2021 08:58:02 +0100
Subject: [PATCH 01/19] Use both, detailed meshes and coarse collision
 geometries

They serve different purposes:
- coarse collision geometries mimic the internal self-collision checking
  of the real-robot controller
- mesh models should be used for collision checking with environmental objects
---
 franka_description/robots/hand.urdf.xacro     |  1 +
 franka_description/robots/hand.xacro          | 17 ++--
 .../robots/panda_arm.urdf.xacro               |  3 +-
 franka_description/robots/panda_arm.xacro     | 96 ++++++++-----------
 franka_description/robots/utils.xacro         | 34 +++++++
 5 files changed, 82 insertions(+), 69 deletions(-)

diff --git a/franka_description/robots/hand.urdf.xacro b/franka_description/robots/hand.urdf.xacro
index 08d3899b..fe9f9e02 100644
--- a/franka_description/robots/hand.urdf.xacro
+++ b/franka_description/robots/hand.urdf.xacro
@@ -1,5 +1,6 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
+  <xacro:include filename="utils.xacro" />
   <xacro:include filename="hand.xacro"/>
   <xacro:hand arm_id="panda" safety_distance="0.03"/>
 </robot>
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 2ed482de..756789ce 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/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="hand" params="connected_to:='' arm_id:='panda' rpy:='0 0 0' xyz:='0 0 0' safety_distance:=0">
+  <xacro:macro name="hand" params="connected_to:='' arm_id:='panda' rpy:='0 0 0' xyz:='0 0 0' safety_distance:=0 description_pkg:=franka_description">
     <xacro:unless value="${connected_to == ''}">
       <joint name="${arm_id}_hand_joint" type="fixed">
         <parent link="${connected_to}"/>
@@ -9,12 +9,8 @@
         <origin xyz="${xyz}" rpy="${rpy}"/>
       </joint>
     </xacro:unless>
-    <link name="${arm_id}_hand">
-      <visual>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/hand.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="hand">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
         <geometry>
@@ -51,7 +47,8 @@
           <sphere radius="${0.02+safety_distance}"  />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <!-- Define the hand_tcp frame -->
     <link name="${arm_id}_hand_tcp" />
     <joint name="${arm_id}_hand_tcp_joint" type="fixed">
@@ -62,7 +59,7 @@
     <link name="${arm_id}_leftfinger">
       <visual>
         <geometry>
-          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+          <mesh filename="package://${description_pkg}/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
     </link>
@@ -70,7 +67,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
         <geometry>
-          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+          <mesh filename="package://${description_pkg}/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
    </link>
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index ce154776..fee7cc86 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -10,7 +10,8 @@
 
   <xacro:unless value="$(arg gazebo)">
     <!-- Create a URDF for a real hardware -->
-    <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
+    <xacro:include filename="utils.xacro" />
+    <xacro:include filename="panda_arm.xacro" />
     <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
 
     <xacro:if value="$(arg hand)">
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index b44b230a..21871f78 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -10,12 +10,8 @@
         <origin rpy="${rpy}" xyz="${xyz}"/>
       </joint>
     </xacro:unless>
-    <link name="${arm_id}_link0">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link0.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link0">
+      <self_collision_geometries>
       <collision>
         <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0"/>
         <geometry>
@@ -34,13 +30,10 @@
           <sphere radius="${0.06+safety_distance}" />
         </geometry>
       </collision>
-    </link>
-    <link name="${arm_id}_link1">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link1.dae"/>
-        </geometry>
-      </visual>
+      </self_collision_geometries>
+    </xacro:link>
+    <xacro:link name="link1">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.1915" rpy="0 0 0"/>
         <geometry>
@@ -59,7 +52,8 @@
           <sphere radius="${0.06+safety_distance}" />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint1" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="0 0 0" xyz="0 0 0.333"/>
@@ -68,12 +62,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
     </joint>
-    <link name="${arm_id}_link2">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link2.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link2">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
@@ -92,7 +82,8 @@
           <sphere radius="${0.06+safety_distance}" />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint2" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
       <origin rpy="${-pi/2} 0 0" xyz="0 0 0"/>
@@ -101,12 +92,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
     </joint>
-    <link name="${arm_id}_link3">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link3.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link3">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.145" rpy="0 0 0"/>
         <geometry>
@@ -125,7 +112,8 @@
           <sphere radius="${0.06+safety_distance}" />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint3" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0"/>
@@ -134,12 +122,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
     </joint>
-    <link name="${arm_id}_link4">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link4.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link4">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
@@ -158,7 +142,8 @@
           <sphere radius="${0.06+safety_distance}" />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint4" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
       <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0"/>
@@ -167,12 +152,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
     </joint>
-    <link name="${arm_id}_link5">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link5.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link5">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.26" rpy="0 0 0"/>
         <geometry>
@@ -209,8 +190,8 @@
           <sphere radius="${0.025+safety_distance}" />
         </geometry>
       </collision>
-
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint5" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0"/>
@@ -219,12 +200,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
     </joint>
-    <link name="${arm_id}_link6">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link6.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link6">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.03" rpy="0 0 0"/>
         <geometry>
@@ -243,7 +220,8 @@
           <sphere radius="${0.05+safety_distance}" />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint6" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
       <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
@@ -252,12 +230,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
     </joint>
-    <link name="${arm_id}_link7">
-      <visual>
-        <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/link7.dae"/>
-        </geometry>
-      </visual>
+    <xacro:link name="link7">
+      <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0.01" rpy="0 0 0"/>
         <geometry>
@@ -276,7 +250,8 @@
           <sphere radius="${0.04+safety_distance}"  />
         </geometry>
       </collision>
-    </link>
+      </self_collision_geometries>
+    </xacro:link>
     <joint name="${arm_id}_joint7" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
@@ -285,7 +260,8 @@
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
     </joint>
-    <link name="${arm_id}_link8">
+    <link name="${arm_id}_link8"/>
+    <link name="${arm_id}_link8_sc">
       <collision>
         <origin xyz="0.0424 0.0424 -0.0250" rpy="${pi} ${pi/2} ${pi/2}"/>
         <geometry>
@@ -305,6 +281,10 @@
         </geometry>
       </collision>
     </link>
+    <joint name="${arm_id}_link8_sc_joint" type="fixed">
+      <parent link="${arm_id}_link8" />
+      <child link="${arm_id}_link8_sc" />
+    </joint>
     <joint name="${arm_id}_joint8" type="fixed">
       <origin rpy="0 0 0" xyz="0 0 0.107"/>
       <parent link="${arm_id}_link7"/>
diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index d6b53a1a..32ee43ae 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -1,6 +1,40 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <!-- ========================================================== -->
+  <!-- Macro to add a single link, both with                      -->
+  <!--  * detailed meshes for environmental collision checking    -->
+  <!--  * and coarse geometry models for self-collision checking  -->
+  <!--                                                            -->
+  <!-- name:  Name of the robot link (without prefix)             -->
+  <!-- self_collision_geometries:  self <collision> models        -->
+  <!-- ========================================================== -->
+  <xacro:macro name="link" params="name prefix=${arm_id}_ **self_collision_geometries">
+    <!-- sub-link defining detailed meshes for collision with environment -->
+    <link name="${prefix}${name}">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/${name}.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/collision/${name}.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
+    <link name="${prefix}${name}_sc">
+      <xacro:unless value="$(arg gazebo)">
+        <xacro:insert_block name="self_collision_geometries" />
+      </xacro:unless>
+    </link>
+    <!-- fixed joint between both sub-links -->
+    <joint name="${prefix}${name}_sc_joint" type="fixed">
+      <parent link="${prefix}${name}" />
+      <child link="${prefix}${name}_sc" />
+    </joint>
+  </xacro:macro>
 
   <!-- ========================================================== -->
   <!-- Adds the required tags to simulate one joint in gazebo     -->

From a5e663e091c2e5f9034f65e2e024ca6a4d5edcb7 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 12 Nov 2021 09:34:26 +0100
Subject: [PATCH 02/19] Unify real-world and Gazebo URDF descriptions

- Declare inertial properties in a yaml file
- Use xacro:inertial macro
---
 franka_description/robots/hand.xacro          | 150 +++++--
 franka_description/robots/inertial.yaml       | 137 +++++++
 .../robots/panda_arm.urdf.xacro               |  42 +-
 franka_description/robots/panda_arm.xacro     |   7 +
 franka_description/robots/panda_gazebo.xacro  | 388 ------------------
 franka_description/robots/utils.xacro         |  22 +
 6 files changed, 291 insertions(+), 455 deletions(-)
 create mode 100644 franka_description/robots/inertial.yaml
 delete mode 100644 franka_description/robots/panda_gazebo.xacro

diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 756789ce..85f1a47d 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -4,87 +4,147 @@
   <xacro:macro name="hand" params="connected_to:='' arm_id:='panda' rpy:='0 0 0' xyz:='0 0 0' safety_distance:=0 description_pkg:=franka_description">
     <xacro:unless value="${connected_to == ''}">
       <joint name="${arm_id}_hand_joint" type="fixed">
-        <parent link="${connected_to}"/>
-        <child link="${arm_id}_hand"/>
-        <origin xyz="${xyz}" rpy="${rpy}"/>
+        <parent link="${connected_to}" />
+        <child link="${arm_id}_hand" />
+        <origin xyz="${xyz}" rpy="${rpy}" />
       </joint>
     </xacro:unless>
     <xacro:link name="hand">
       <self_collision_geometries>
+        <collision>
+          <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}" />
+          <geometry>
+            <cylinder radius="${0.04+safety_distance}" length="0.1" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 -0.05 0.04" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.04+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0.05 0.04" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.04+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}" />
+          <geometry>
+            <cylinder radius="${0.02+safety_distance}" length="0.1" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 -0.05 0.1" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.02+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0.05 0.1" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.02+safety_distance}" />
+          </geometry>
+        </collision>
+      </self_collision_geometries>
+    </xacro:link>
+    <!-- Define the hand_tcp frame -->
+    <link name="${arm_id}_hand_tcp" />
+    <joint name="${arm_id}_hand_tcp_joint" type="fixed">
+      <origin xyz="0 0 0.1034" rpy="0 0 0" />
+      <parent link="${arm_id}_hand" />
+      <child link="${arm_id}_hand_tcp" />
+    </joint>
+    <link name="${arm_id}_leftfinger">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/finger.dae" />
+        </geometry>
+      </visual>
+      <!-- screw mount -->
       <collision>
-        <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
+        <origin xyz="0 18.5e-3 11e-3" rpy="0 0 0" />
         <geometry>
-          <cylinder radius="${0.04+safety_distance}" length="0.1" />
+          <box size="22e-3 15e-3 20e-3" />
         </geometry>
       </collision>
+      <!-- cartriage sledge -->
       <collision>
-        <origin xyz="0 -0.05 0.04" rpy="0 0 0"/>
+        <origin xyz="0 6.8e-3 2.2e-3" rpy="0 0 0" />
         <geometry>
-          <sphere radius="${0.04+safety_distance}"  />
+          <box size="22e-3 8.8e-3 3.8e-3" />
         </geometry>
       </collision>
+      <!-- diagonal finger -->
       <collision>
-        <origin xyz="0 0.05 0.04" rpy="0 0 0"/>
+        <origin xyz="0 15.9e-3 28.35e-3" rpy="${pi/6} 0 0" />
         <geometry>
-          <sphere radius="${0.04+safety_distance}"  />
+          <box size="17.5e-3 7e-3 23.5e-3" />
         </geometry>
       </collision>
+      <!-- rubber tip with which to grasp -->
       <collision>
-        <origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}"/>
+        <origin xyz="0 7.58e-3 45.25e-3" rpy="0 0 0" />
         <geometry>
-          <cylinder radius="${0.02+safety_distance}" length="0.1" />
+          <box size="17.5e-3 15.2e-3 18.5e-3" />
         </geometry>
       </collision>
+      <xacro:inertial name="leftfinger" />
+    </link>
+    <link name="${arm_id}_rightfinger">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/finger.dae" />
+        </geometry>
+      </visual>
+      <!-- screw mount -->
       <collision>
-        <origin xyz="0 -0.05 0.1" rpy="0 0 0"/>
+        <origin xyz="0 -18.5e-3 11e-3" rpy="0 0 0" />
         <geometry>
-          <sphere radius="${0.02+safety_distance}"  />
+          <box size="22e-3 15e-3 20e-3" />
         </geometry>
       </collision>
+      <!-- cartriage sledge -->
       <collision>
-        <origin xyz="0 0.05 0.1" rpy="0 0 0"/>
+        <origin xyz="0 -6.8e-3 2.2e-3" rpy="0 0 0" />
         <geometry>
-          <sphere radius="${0.02+safety_distance}"  />
+          <box size="22e-3 8.8e-3 3.8e-3" />
         </geometry>
       </collision>
-      </self_collision_geometries>
-    </xacro:link>
-    <!-- Define the hand_tcp frame -->
-    <link name="${arm_id}_hand_tcp" />
-    <joint name="${arm_id}_hand_tcp_joint" type="fixed">
-      <origin xyz="0 0 0.1034" rpy="0 0 0" />
-      <parent link="${arm_id}_hand" />
-      <child link="${arm_id}_hand_tcp" />
-    </joint>
-    <link name="${arm_id}_leftfinger">
-      <visual>
+      <!-- diagonal finger -->
+      <collision>
+        <origin xyz="0 -15.9e-3 28.35e-3" rpy="${-pi/6} 0 0" />
         <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/finger.dae"/>
+          <box size="17.5e-3 7e-3 23.5e-3" />
         </geometry>
-      </visual>
-    </link>
-    <link name="${arm_id}_rightfinger">
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+      </collision>
+      <!-- rubber tip with which to grasp -->
+      <collision>
+        <origin xyz="0 -7.58e-3 45.25e-3" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://${description_pkg}/meshes/visual/finger.dae"/>
+          <box size="17.5e-3 15.2e-3 18.5e-3" />
         </geometry>
-      </visual>
-   </link>
+      </collision>
+      <xacro:inertial name="rightfinger" />
+    </link>
     <joint name="${arm_id}_finger_joint1" type="prismatic">
-      <parent link="${arm_id}_hand"/>
-      <child link="${arm_id}_leftfinger"/>
-      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
-      <axis xyz="0 1 0"/>
-      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
+      <parent link="${arm_id}_hand" />
+      <child link="${arm_id}_leftfinger" />
+      <origin xyz="0 0 0.0584" rpy="0 0 0" />
+      <axis xyz="0 1 0" />
+      <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">
-      <parent link="${arm_id}_hand"/>
-      <child link="${arm_id}_rightfinger"/>
-      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
-      <axis xyz="0 -1 0"/>
-      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
+      <parent link="${arm_id}_hand" />
+      <child link="${arm_id}_rightfinger" />
+      <origin xyz="0 0 0.0584" rpy="0 0 0" />
+      <axis xyz="0 -1 0" />
+      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2" />
       <mimic joint="${arm_id}_finger_joint1" />
+      <dynamics damping="0.3" />
     </joint>
   </xacro:macro>
 </robot>
diff --git a/franka_description/robots/inertial.yaml b/franka_description/robots/inertial.yaml
new file mode 100644
index 00000000..e860909d
--- /dev/null
+++ b/franka_description/robots/inertial.yaml
@@ -0,0 +1,137 @@
+# This file does not contain official inertial properties
+# of panda robot. The values are from the identification
+# results published in: Identification of the Franka Emika
+# PandaRobot With Retrieval of Feasible Parameters Using
+# Penalty-Based Optimization
+# by: Claudio Gaz, Marco Cognetti, Alexander Oliva,
+# Paolo Robuffo Giordano, Alessandro de Luca
+link0:
+  origin:
+    xyz: -0.041018 -0.00014 0.049974
+    rpy: 0 0 0
+  mass: 0.629769
+  inertia:
+    xx: 0.00315
+    yy: 0.00388
+    zz: 0.004285
+    xy: 8.2904E-07
+    xz: 0.00015
+    yz: 8.2299E-06
+
+link1:
+  origin:
+    xyz: 0.003875 0.002081 -0.04762
+    rpy: 0 0 0
+  mass: 4.970684
+  inertia:
+    xx: 0.70337
+    yy: 0.70661
+    zz: 0.0091170
+    xy: -0.00013900
+    xz: 0.0067720
+    yz: 0.019169
+
+link2:
+  origin:
+    xyz:  -0.003141 -0.02872  0.003495
+    rpy: 0 0 0
+  mass: 0.646926
+  inertia:
+    xx: 0.0079620
+    yy: 2.8110e-02
+    zz: 2.5995e-02
+    xy: -3.9250e-3
+    xz: 1.0254e-02
+    yz: 7.0400e-04
+
+link3:
+  origin:
+    xyz: 2.7518e-02 3.9252e-02 -6.6502e-02
+    rpy: 0 0 0
+  mass: 3.228604
+  inertia:
+    xx: 3.7242e-02
+    yy: 3.6155e-02
+    zz: 1.0830e-02
+    xy: -4.7610e-03
+    xz: -1.1396e-02
+    yz: -1.2805e-02
+
+link4:
+  origin:
+    xyz: -5.317e-02 1.04419e-01 2.7454e-02
+    rpy: 0 0 0
+  mass: 3.587895
+  inertia:
+    xx: 2.5853e-02
+    yy: 1.9552e-02
+    zz: 2.8323e-02
+    xy: 7.7960e-03
+    xz: -1.3320e-03
+    yz: 8.6410e-03
+
+link5:
+  origin:
+    xyz: -1.1953e-02 4.1065e-02 -3.8437e-02
+    rpy: 0 0 0
+  mass: 1.225946
+  inertia:
+    xx: 3.5549e-02
+    yy: 2.9474e-02
+    zz: 8.6270e-03
+    xy: -2.1170e-03
+    xz: -4.0370e-03
+    yz: 2.2900e-04
+
+link6:
+  origin:
+    xyz: 6.0149e-02 -1.4117e-02 -1.0517e-02
+    rpy: 0 0 0
+  mass: 1.666555
+  inertia:
+    xx: 1.9640e-03
+    yy: 4.3540e-03
+    zz: 5.4330e-03
+    xy: 1.0900e-04
+    xz: -1.1580e-03
+    yz: 3.4100e-04
+
+link7:
+  origin:
+    xyz: 1.0517e-02 -4.252e-03 6.1597e-02
+    rpy: 0 0 0
+  mass: 7.35522e-01
+  inertia:
+    xx: 1.2516e-02
+    yy: 1.0027e-02
+    zz: 4.8150e-03
+    xy: -4.2800e-04
+    xz: -1.1960e-03
+    yz: -7.4100e-04
+
+hand:
+  origin:
+    xyz: -0.01 0 0.03
+    rpy: 0 0 0
+  mass: 0.73
+  inertia:
+    xx: 0.001
+    yy: 0.0025
+    zz: 0.0017
+    xy: 0
+    xz: 0
+    yz: 0
+
+leftfinger: &finger
+  origin:
+    xyz: 0 0 0
+    rpy: 0 0 0
+  mass: 0.015
+  inertia:
+    xx: 2.3749999999999997e-06
+    yy: 2.3749999999999997e-06
+    zz: 7.5e-07
+    xy: 0
+    xz: 0
+    yz: 0
+rightfinger: *finger
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index fee7cc86..c61d48ff 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -8,35 +8,25 @@
   <!-- Is the robot being simulated in gazebo?" -->
   <xacro:arg name="gazebo" default="false" />
 
-  <xacro:unless value="$(arg gazebo)">
-    <!-- Create a URDF for a real hardware -->
-    <xacro:include filename="utils.xacro" />
-    <xacro:include filename="panda_arm.xacro" />
-    <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
+  <!-- Load interial properties. Property is implicitly passed to macros. -->
+  <xacro:property name="inertial" value="${xacro.load_yaml('inertial.yaml')}" />
 
-    <xacro:if value="$(arg hand)">
-      <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
-      <xacro:hand arm_id="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
-    </xacro:if>
-  </xacro:unless>
+  <xacro:include filename="utils.xacro" />
+  <xacro:include filename="panda_arm.xacro" />
 
+  <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
+
+  <xacro:if value="$(arg hand)">
+    <xacro:include filename="hand.xacro"/>
+    <xacro:hand arm_id="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
+  </xacro:if>
+
+  <!-- Define additional Gazebo tags -->
   <xacro:if value="$(arg gazebo)">
 
     <xacro:arg name="xyz" default="0 0 0" />
     <xacro:arg name="rpy" default="0 0 0" />
 
-    <!-- Create a simulatable URDF -->
-    <xacro:include filename="$(find franka_description)/robots/utils.xacro" />
-    <xacro:include filename="$(find franka_description)/robots/panda_gazebo.xacro" />
-
-    <xacro:panda_arm arm_id="$(arg arm_id)" />
-
-    <xacro:if value="$(arg hand)">
-      <xacro:hand arm_id="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" />
-      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
-      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
-    </xacro:if>
-
     <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
     <link name="world" />
     <joint name="world_joint" type="fixed">
@@ -75,6 +65,14 @@
        tip="$(arg arm_id)_joint8"
      />
 
+    <xacro:if value="$(arg hand)">
+      <xacro:gazebo-joint joint="${prefix}finger_joint1" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:gazebo-joint joint="${prefix}finger_joint2" transmission="hardware_interface/EffortJointInterface" />
+      <!-- Friction specific material for Rubber/Rubber contact -->
+      <xacro:gazebo-friction link="${prefix}leftfinger" mu="1.13" />
+      <xacro:gazebo-friction link="${prefix}rightfinger" mu="1.13" />
+    </xacro:if>
+
     <gazebo>
       <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
         <controlPeriod>0.001</controlPeriod>
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 21871f78..c5687165 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -61,6 +61,7 @@
       <child link="${arm_id}_link1"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <xacro:link name="link2">
       <self_collision_geometries>
@@ -91,6 +92,7 @@
       <child link="${arm_id}_link2"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <xacro:link name="link3">
       <self_collision_geometries>
@@ -121,6 +123,7 @@
       <child link="${arm_id}_link3"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <xacro:link name="link4">
       <self_collision_geometries>
@@ -151,6 +154,7 @@
       <child link="${arm_id}_link4"/>
       <axis xyz="0 0 1"/>
       <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <xacro:link name="link5">
       <self_collision_geometries>
@@ -199,6 +203,7 @@
       <child link="${arm_id}_link5"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <xacro:link name="link6">
       <self_collision_geometries>
@@ -229,6 +234,7 @@
       <child link="${arm_id}_link6"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <xacro:link name="link7">
       <self_collision_geometries>
@@ -259,6 +265,7 @@
       <child link="${arm_id}_link7"/>
       <axis xyz="0 0 1"/>
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
     <link name="${arm_id}_link8"/>
     <link name="${arm_id}_link8_sc">
diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro
deleted file mode 100644
index 085c19bc..00000000
--- a/franka_description/robots/panda_gazebo.xacro
+++ /dev/null
@@ -1,388 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
-
-  <!-- =============================================================== -->
-  <!-- || This file does not contain official inertial properties   || -->
-  <!-- || of panda robot. The values are from the identification    || -->
-  <!-- || results published in: Identification of the Franka Emika  || -->
-  <!-- || PandaRobot With Retrieval of Feasible Parameters Using    || -->
-  <!-- || Penalty-Based Optimization                                || -->
-  <!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva,         || -->
-  <!-- || Paolo Robuffo Giordano, Alessandro de Luca                || -->
-  <!-- =============================================================== -->
-
-  <xacro:macro name="panda_arm" params="arm_id:='panda' connected_to:='' xyz:='0 0 0' rpy:='0 0 0'">
-    <xacro:unless value="${not connected_to}">
-      <joint name="${arm_id}_joint_${connected_to}" type="fixed">
-        <parent link="${connected_to}"/>
-        <child link="${arm_id}_link0"/>
-        <origin rpy="${rpy}" xyz="${xyz}"/>
-      </joint>
-    </xacro:unless>
-
-    <link name="${arm_id}_link0">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <!-- Estimated guesses from CAD -->
-        <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
-        <mass value="0.629769"/>
-        <inertia ixx="0.00315" ixy="8.2904E-07" ixz="0.00015"
-                               iyy="0.00388"    iyz="8.2299E-06"
-                                                izz="0.004285" />
-      </inertial>
-    </link>
-
-    <link name="${arm_id}_link1">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
-        <mass value="4.970684"/>
-        <inertia ixx="0.70337" ixy="-0.00013900" ixz="0.0067720"
-                               iyy="0.70661"     iyz="0.019169"
-                                                 izz="0.0091170" />
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint1" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="0 0 0" xyz="0 0 0.333"/>
-      <parent link="${arm_id}_link0"/>
-      <child link="${arm_id}_link1"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="15" lower="-2.9671" effort="87" upper="2.9671" velocity="2.5"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link2">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz=" -0.003141 -0.02872  0.003495"/>
-        <mass value="0.646926"/>
-        <inertia ixx="0.0079620" ixy="-3.9250e-3" ixz="1.0254e-02"
-                                 iyy="2.8110e-02" iyz="7.0400e-04"
-                                                  izz="2.5995e-02" />
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint2" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
-      <origin rpy="${-pi/2} 0 0" xyz="0 0 0"/>
-      <parent link="${arm_id}_link1"/>
-      <child link="${arm_id}_link2"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="7.5" lower="-1.8326" effort="87" upper="1.8326" velocity="2.5"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link3">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
-        <mass value="3.228604"/>
-        <inertia ixx="3.7242e-02" ixy="-4.7610e-03" ixz="-1.1396e-02"
-                                  iyy="3.6155e-02"  iyz="-1.2805e-02"
-                                                    izz="1.0830e-02" />
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint3" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0"/>
-      <parent link="${arm_id}_link2"/>
-      <child link="${arm_id}_link3"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="12.5" lower="-2.9671" effort="87" upper="2.9671" velocity="2.5"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link4">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
-        <mass value="3.587895"/>
-        <inertia ixx="2.5853e-02" ixy="7.7960e-03" ixz="-1.3320e-03"
-                                  iyy="1.9552e-02" iyz="8.6410e-03"
-                                                   izz="2.8323e-02" />
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint4" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
-      <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0"/>
-      <parent link="${arm_id}_link3"/>
-      <child link="${arm_id}_link4"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="12.5" lower="-3.1416" effort="87" upper="-0.4" velocity="2.5"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link5">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
-        <mass value="1.225946"/>
-        <inertia ixx="3.5549e-02" ixy="-2.1170e-03" ixz="-4.0370e-03"
-                                  iyy="2.9474e-02"  iyz="2.2900e-04"
-                                                    izz="8.6270e-03" />
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint5" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0"/>
-      <parent link="${arm_id}_link4"/>
-      <child link="${arm_id}_link5"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="15" lower="-2.9671" effort="12" upper="2.9671" velocity="3"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link6">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
-        <mass value="1.666555"/>
-        <inertia ixx="1.9640e-03" ixy="1.0900e-04" ixz="-1.1580e-03"
-                                  iyy="4.3540e-03" iyz="3.4100e-04"
-                                                   izz="5.4330e-03" />
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint6" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
-      <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
-      <parent link="${arm_id}_link5"/>
-      <child link="${arm_id}_link6"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="20" lower="-0.0873" effort="12" upper="3.8223" velocity="3"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link7">
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
-        </geometry>
-      </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
-        <mass value="7.35522e-01"/>
-        <inertia ixx="1.2516e-02" ixy="-4.2800e-04" ixz="-1.1960e-03"
-                                  iyy="1.0027e-02"  iyz="-7.4100e-04"
-                                                    izz="4.8150e-03"/>
-      </inertial>
-    </link>
-
-    <joint name="${arm_id}_joint7" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
-      <parent link="${arm_id}_link6"/>
-      <child link="${arm_id}_link7"/>
-      <axis xyz="0 0 1"/>
-      <limit acceleration="20" lower="-2.9671" effort="12" upper="2.9671" velocity="3"/>
-      <dynamics D="1" K="7000" mu_coulomb="0" mu_viscous="16" damping="0.003" friction="0.0"/>
-    </joint>
-
-    <link name="${arm_id}_link8" />
-
-    <joint name="${arm_id}_joint8" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0.107"/>
-      <parent link="${arm_id}_link7"/>
-      <child link="${arm_id}_link8"/>
-      <axis xyz="0 0 0"/>
-    </joint>
-
-  </xacro:macro>
-
-  <xacro:macro name="hand" params="connected_to:='' arm_id:='' xyz:='0 0 0' rpy:='0 0 0'">
-    <xacro:include filename="$(find franka_description)/robots/utils.xacro"/>
-
-    <xacro:unless value="${connected_to == ''}">
-      <joint name="${arm_id}_hand_joint" type="fixed">
-        <parent link="${connected_to}"/>
-        <child link="${arm_id}_hand"/>
-        <origin xyz="${xyz}" rpy="${rpy}"/>
-      </joint>
-    </xacro:unless>
-
-    <link name="${arm_id}_hand">
-      <visual>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/hand.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/hand.stl" />
-        </geometry>
-      </collision>
-      <inertial>
-        <mass value="0.73" />
-        <origin xyz="-0.01 0 0.03" />
-        <inertia ixx="0.001" ixy="0"      ixz="0"
-                             iyy="0.0025" iyz="0"
-                                          izz="0.0017" />
-      </inertial>
-    </link>
-
-    <xacro:macro name="finger-collision" params="sign">
-      <!-- screw mount -->
-      <collision>
-        <origin xyz="0 ${sign}18.5e-3 11e-3" rpy="0 0 0"/>
-        <geometry>
-          <box size="22e-3 15e-3 20e-3" />
-        </geometry>
-      </collision>
-      <!-- cartriage sledge -->
-      <collision>
-        <origin xyz="0 ${sign}6.8e-3 2.2e-3" rpy="0 0 0"/>
-        <geometry>
-          <box size="22e-3 8.8e-3 3.8e-3" />
-        </geometry>
-      </collision>
-      <!-- diagonal finger -->
-      <collision>
-        <origin xyz="0 ${sign}15.9e-3 28.35e-3" rpy="${sign}${pi/6} 0 0"/>
-        <geometry>
-          <box size="17.5e-3 7e-3 23.5e-3" />
-        </geometry>
-      </collision>
-      <!-- rubber tip with which to grasp -->
-      <collision>
-        <origin xyz="0 ${sign}7.58e-3 45.25e-3" rpy="0 0 0"/>
-        <geometry>
-          <box size="17.5e-3 15.2e-3 18.5e-3" />
-        </geometry>
-      </collision>
-    </xacro:macro>
-
-    <link name="${arm_id}_leftfinger">
-      <xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
-      <visual>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
-        </geometry>
-      </visual>
-      <xacro:finger-collision sign="+" />
-    </link>
-
-    <link name="${arm_id}_rightfinger">
-      <xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
-        </geometry>
-      </visual>
-      <xacro:finger-collision sign="-" />
-    </link>
-
-    <!-- Friction specific material for Rubber/Rubber contact -->
-    <!-- See: -->
-    <xacro:gazebo-friction link="${arm_id}_leftfinger" mu="1.13" />
-    <xacro:gazebo-friction link="${arm_id}_rightfinger" mu="1.13" />
-
-    <joint name="${arm_id}_finger_joint1" type="prismatic">
-      <parent link="${arm_id}_hand"/>
-      <child link="${arm_id}_leftfinger"/>
-      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
-      <axis xyz="0 1 0"/>
-      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
-      <dynamics friction="0.0" damping="0.03"/>
-    </joint>
-
-    <joint name="${arm_id}_finger_joint2" type="prismatic">
-      <parent link="${arm_id}_hand"/>
-      <child link="${arm_id}_rightfinger"/>
-      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
-      <axis xyz="0 -1 0"/>
-      <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
-      <dynamics friction="0.0" damping="0.03"/>
-      <mimic joint="${arm_id}_finger_joint1" />
-    </joint>
-  </xacro:macro>
-</robot>
diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index 32ee43ae..d66fdb87 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -1,5 +1,26 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- ============================================================ -->
+  <!-- Macro to add an <interial> tag based on yaml-load properties -->
+  <!--                                                              -->
+  <!-- name:     Name of the robot link (without prefix)            -->
+  <!-- inertial: Dictionary of inertia properties                   -->
+  <!-- ============================================================ -->
+  <xacro:macro name="inertial" params="inertial:=^ name:=^">
+    <xacro:if value="$(arg gazebo)">
+      <xacro:unless value="${name in inertial}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
+      <xacro:if value="${name in inertial}">
+        <!-- Access inertia properties of link 'name' -->
+        <xacro:property name="inertial" value="${inertial[name]}" lazy_eval="false" />
+        <inertial>
+          <origin rpy="${inertial.origin.rpy}" xyz="${inertial.origin.xyz}" />
+          <mass value="${inertial.mass}" />
+          <xacro:property name="I" value="${inertial.inertia}" />
+          <inertia ixx="${I.xx}" ixy="${I.xy}" ixz="${I.xz}" iyy="${I.yy}" iyz="${I.yz}" izz="${I.zz}" />
+        </inertial>
+      </xacro:if>
+    </xacro:if>
+  </xacro:macro>
 
   <!-- ========================================================== -->
   <!-- Macro to add a single link, both with                      -->
@@ -22,6 +43,7 @@
           <mesh filename="package://franka_description/meshes/collision/${name}.stl" />
         </geometry>
       </collision>
+      <xacro:inertial />
     </link>
     <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
     <link name="${prefix}${name}_sc">

From 0c48a4c1f1ef994a2c2908a52b15b94a4edfbdb0 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 14 Mar 2022 19:13:34 +0100
Subject: [PATCH 03/19] Restore Melodic compatibility

xacro requires macro names to be different from actual tag names.
---
 franka_description/robots/hand.xacro      |  8 +++---
 franka_description/robots/panda_arm.xacro | 32 +++++++++++------------
 franka_description/robots/utils.xacro     |  6 ++---
 3 files changed, 23 insertions(+), 23 deletions(-)

diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 85f1a47d..5feea387 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -9,7 +9,7 @@
         <origin xyz="${xyz}" rpy="${rpy}" />
       </joint>
     </xacro:unless>
-    <xacro:link name="hand">
+    <xacro:link_detailed_coarse name="hand">
       <self_collision_geometries>
         <collision>
           <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}" />
@@ -48,7 +48,7 @@
           </geometry>
         </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <!-- Define the hand_tcp frame -->
     <link name="${arm_id}_hand_tcp" />
     <joint name="${arm_id}_hand_tcp_joint" type="fixed">
@@ -90,7 +90,7 @@
           <box size="17.5e-3 15.2e-3 18.5e-3" />
         </geometry>
       </collision>
-      <xacro:inertial name="leftfinger" />
+      <xacro:inertial_props name="leftfinger" />
     </link>
     <link name="${arm_id}_rightfinger">
       <visual>
@@ -127,7 +127,7 @@
           <box size="17.5e-3 15.2e-3 18.5e-3" />
         </geometry>
       </collision>
-      <xacro:inertial name="rightfinger" />
+      <xacro:inertial_props name="rightfinger" />
     </link>
     <joint name="${arm_id}_finger_joint1" type="prismatic">
       <parent link="${arm_id}_hand" />
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index c5687165..06b9d0ca 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -10,7 +10,7 @@
         <origin rpy="${rpy}" xyz="${xyz}"/>
       </joint>
     </xacro:unless>
-    <xacro:link name="link0">
+    <xacro:link_detailed_coarse name="link0">
       <self_collision_geometries>
       <collision>
         <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0"/>
@@ -31,8 +31,8 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
-    <xacro:link name="link1">
+    </xacro:link_detailed_coarse>
+    <xacro:link_detailed_coarse name="link1">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.1915" rpy="0 0 0"/>
@@ -53,7 +53,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint1" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="0 0 0" xyz="0 0 0.333"/>
@@ -63,7 +63,7 @@
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
-    <xacro:link name="link2">
+    <xacro:link_detailed_coarse name="link2">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -84,7 +84,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint2" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
       <origin rpy="${-pi/2} 0 0" xyz="0 0 0"/>
@@ -94,7 +94,7 @@
       <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
-    <xacro:link name="link3">
+    <xacro:link_detailed_coarse name="link3">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.145" rpy="0 0 0"/>
@@ -115,7 +115,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint3" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0"/>
@@ -125,7 +125,7 @@
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
-    <xacro:link name="link4">
+    <xacro:link_detailed_coarse name="link4">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -146,7 +146,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint4" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
       <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0"/>
@@ -156,7 +156,7 @@
       <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
-    <xacro:link name="link5">
+    <xacro:link_detailed_coarse name="link5">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.26" rpy="0 0 0"/>
@@ -195,7 +195,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint5" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0"/>
@@ -205,7 +205,7 @@
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
-    <xacro:link name="link6">
+    <xacro:link_detailed_coarse name="link6">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 -0.03" rpy="0 0 0"/>
@@ -226,7 +226,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint6" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
       <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
@@ -236,7 +236,7 @@
       <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
-    <xacro:link name="link7">
+    <xacro:link_detailed_coarse name="link7">
       <self_collision_geometries>
       <collision>
         <origin xyz="0 0 0.01" rpy="0 0 0"/>
@@ -257,7 +257,7 @@
         </geometry>
       </collision>
       </self_collision_geometries>
-    </xacro:link>
+    </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint7" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index d66fdb87..fba3d2c3 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -6,7 +6,7 @@
   <!-- name:     Name of the robot link (without prefix)            -->
   <!-- inertial: Dictionary of inertia properties                   -->
   <!-- ============================================================ -->
-  <xacro:macro name="inertial" params="inertial:=^ name:=^">
+  <xacro:macro name="inertial_props" params="inertial:=^ name:=^">
     <xacro:if value="$(arg gazebo)">
       <xacro:unless value="${name in inertial}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
       <xacro:if value="${name in inertial}">
@@ -30,7 +30,7 @@
   <!-- name:  Name of the robot link (without prefix)             -->
   <!-- self_collision_geometries:  self <collision> models        -->
   <!-- ========================================================== -->
-  <xacro:macro name="link" params="name prefix=${arm_id}_ **self_collision_geometries">
+  <xacro:macro name="link_detailed_coarse" params="name prefix=${arm_id}_ **self_collision_geometries">
     <!-- sub-link defining detailed meshes for collision with environment -->
     <link name="${prefix}${name}">
       <visual>
@@ -43,7 +43,7 @@
           <mesh filename="package://franka_description/meshes/collision/${name}.stl" />
         </geometry>
       </collision>
-      <xacro:inertial />
+      <xacro:inertial_props />
     </link>
     <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
     <link name="${prefix}${name}_sc">

From 18f027cbba640accbdc8965dc3883c71fb1ccea3 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 15 Mar 2022 08:07:55 +0100
Subject: [PATCH 04/19] Autoformatting to fix indentation

---
 franka_description/robots/panda_arm.xacro | 408 +++++++++++-----------
 1 file changed, 204 insertions(+), 204 deletions(-)

diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 06b9d0ca..4a42bf4a 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -12,250 +12,250 @@
     </xacro:unless>
     <xacro:link_detailed_coarse name="link0">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0"/>
-        <geometry>
-          <cylinder radius="${0.06+safety_distance}" length="0.03" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="-0.06 0 0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="-0.09 0 0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0" />
+          <geometry>
+            <cylinder radius="${0.06+safety_distance}" length="0.03" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="-0.06 0 0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="-0.09 0 0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <xacro:link_detailed_coarse name="link1">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 -0.1915" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.06+safety_distance}" length="0.2830" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.333" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.05" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 -0.1915" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.06+safety_distance}" length="0.2830" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.333" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.05" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint1" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="0 0 0" xyz="0 0 0.333"/>
-      <parent link="${arm_id}_link0"/>
-      <child link="${arm_id}_link1"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
-      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
+      <origin rpy="0 0 0" xyz="0 0 0.333" />
+      <parent link="${arm_id}_link0" />
+      <child link="${arm_id}_link1" />
+      <axis xyz="0 0 1" />
+      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750" />
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
     <xacro:link_detailed_coarse name="link2">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.06+safety_distance}" length="0.12" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.06+safety_distance}" length="0.12" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint2" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
-      <origin rpy="${-pi/2} 0 0" xyz="0 0 0"/>
-      <parent link="${arm_id}_link1"/>
-      <child link="${arm_id}_link2"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
-      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628" />
+      <origin rpy="${-pi/2} 0 0" xyz="0 0 0" />
+      <parent link="${arm_id}_link1" />
+      <child link="${arm_id}_link2" />
+      <axis xyz="0 0 1" />
+      <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750" />
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
     <xacro:link_detailed_coarse name="link3">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 -0.145" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.06+safety_distance}" length="0.15" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.22" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.07" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 -0.145" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.06+safety_distance}" length="0.15" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.22" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.07" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint3" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0"/>
-      <parent link="${arm_id}_link2"/>
-      <child link="${arm_id}_link3"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
-      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
+      <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0" />
+      <parent link="${arm_id}_link2" />
+      <child link="${arm_id}_link3" />
+      <axis xyz="0 0 1" />
+      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750" />
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
     <xacro:link_detailed_coarse name="link4">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.06+safety_distance}" length="0.12" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.06+safety_distance}" length="0.12" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint4" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
-      <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0"/>
-      <parent link="${arm_id}_link3"/>
-      <child link="${arm_id}_link4"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
-      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698" />
+      <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0" />
+      <parent link="${arm_id}_link3" />
+      <child link="${arm_id}_link4" />
+      <axis xyz="0 0 1" />
+      <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750" />
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
     <xacro:link_detailed_coarse name="link5">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 -0.26" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.06+safety_distance}" length="0.1" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.31" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.21" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.06+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0.08 -0.13" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.025+safety_distance}" length="0.14" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0.08 -0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.025+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0.08 -0.20" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.025+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 -0.26" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.06+safety_distance}" length="0.1" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.31" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.21" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.06+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0.08 -0.13" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.025+safety_distance}" length="0.14" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0.08 -0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.025+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0.08 -0.20" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.025+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint5" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-      <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0"/>
-      <parent link="${arm_id}_link4"/>
-      <child link="${arm_id}_link5"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
-      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
+      <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0" />
+      <parent link="${arm_id}_link4" />
+      <child link="${arm_id}_link5" />
+      <axis xyz="0 0 1" />
+      <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100" />
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
     <xacro:link_detailed_coarse name="link6">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 -0.03" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.05+safety_distance}" length="0.08" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 0.01" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.05+safety_distance}" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.07" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.05+safety_distance}" />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 -0.03" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.05+safety_distance}" length="0.08" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 0.01" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.05+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.07" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.05+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint6" type="revolute">
-      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
-      <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
-      <parent link="${arm_id}_link5"/>
-      <child link="${arm_id}_link6"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
-      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525" />
+      <origin rpy="${pi/2} 0 0" xyz="0 0 0" />
+      <parent link="${arm_id}_link5" />
+      <child link="${arm_id}_link6" />
+      <axis xyz="0 0 1" />
+      <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100" />
+      <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
     <xacro:link_detailed_coarse name="link7">
       <self_collision_geometries>
-      <collision>
-        <origin xyz="0 0 0.01" rpy="0 0 0"/>
-        <geometry>
-          <cylinder radius="${0.04+safety_distance}" length="0.14" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 0.08" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.04+safety_distance}"  />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0 -0.06" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.04+safety_distance}"  />
-        </geometry>
-      </collision>
+        <collision>
+          <origin xyz="0 0 0.01" rpy="0 0 0" />
+          <geometry>
+            <cylinder radius="${0.04+safety_distance}" length="0.14" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 0.08" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.04+safety_distance}" />
+          </geometry>
+        </collision>
+        <collision>
+          <origin xyz="0 0 -0.06" rpy="0 0 0" />
+          <geometry>
+            <sphere radius="${0.04+safety_distance}" />
+          </geometry>
+        </collision>
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
     <joint name="${arm_id}_joint7" type="revolute">

From 365e94f904594fabda777ccea5c4158e03c5c15a Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 15 Mar 2022 08:45:51 +0100
Subject: [PATCH 05/19] fixup! Unify real-world and Gazebo URDF descriptions

---
 franka_description/robots/panda_arm.urdf.xacro | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index c61d48ff..2f6a6bb9 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -66,11 +66,11 @@
      />
 
     <xacro:if value="$(arg hand)">
-      <xacro:gazebo-joint joint="${prefix}finger_joint1" transmission="hardware_interface/EffortJointInterface" />
-      <xacro:gazebo-joint joint="${prefix}finger_joint2" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
       <!-- Friction specific material for Rubber/Rubber contact -->
-      <xacro:gazebo-friction link="${prefix}leftfinger" mu="1.13" />
-      <xacro:gazebo-friction link="${prefix}rightfinger" mu="1.13" />
+      <xacro:gazebo-friction link="$(arg arm_id)_leftfinger" mu="1.13" />
+      <xacro:gazebo-friction link="$(arg arm_id)_rightfinger" mu="1.13" />
     </xacro:if>
 
     <gazebo>

From b640dbdb2276e0e055308f7c12b5ee63268e4b9d Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Tue, 12 Apr 2022 16:40:57 +0200
Subject: [PATCH 06/19] ADD: `rpy` parameter to `<xacro:link_detailed_coarse>`

This way you can specify a rotational offset between panda_link* and panda_link*_sc
---
 franka_description/robots/utils.xacro | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index fba3d2c3..e3da5f63 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -28,9 +28,10 @@
   <!--  * and coarse geometry models for self-collision checking  -->
   <!--                                                            -->
   <!-- name:  Name of the robot link (without prefix)             -->
+  <!-- rpy:   Rotation of the *_sc link relative to parent [rad]  -->
   <!-- self_collision_geometries:  self <collision> models        -->
   <!-- ========================================================== -->
-  <xacro:macro name="link_detailed_coarse" params="name prefix=${arm_id}_ **self_collision_geometries">
+  <xacro:macro name="link_detailed_coarse" params="name prefix=${arm_id}_ rpy:='0 0 0' **self_collision_geometries">
     <!-- sub-link defining detailed meshes for collision with environment -->
     <link name="${prefix}${name}">
       <visual>
@@ -53,6 +54,7 @@
     </link>
     <!-- fixed joint between both sub-links -->
     <joint name="${prefix}${name}_sc_joint" type="fixed">
+      <origin rpy="${rpy}" />
       <parent link="${prefix}${name}" />
       <child link="${prefix}${name}_sc" />
     </joint>

From 78608842a5105f85bd789828e87c2ac86683393f Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Tue, 12 Apr 2022 16:41:57 +0200
Subject: [PATCH 07/19] ADD: `<xacro:collision_capsule>` macro

This macro abstracts the creation of a "collision capsule" from two collision
spheres and a collision cylinder.

Since arbitraty RPY rotations are hard to calculate in XACRO (would require
rotation matrix multiplication and probably reduce redability) for now we only
allow the discrete 6 orientations which are aligned with X/Y/Z axes.
---
 franka_description/robots/utils.xacro | 42 +++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index e3da5f63..7ba40aea 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -60,6 +60,48 @@
     </joint>
   </xacro:macro>
 
+  <!-- =========================================================== -->
+  <!-- Add a <collision> tag with a capsule, made from a cylinder  -->
+  <!-- with two spheres at its caps. The capsule will always be    -->
+  <!-- aligned with the axis of 'direction' you pass along.        -->
+  <!--                                                             -->
+  <!-- radius: Radii of both the cylinder and both spheres [m]     -->
+  <!-- length: Length of the cylinder/distance between the centers -->
+  <!--         of the spheres. NOT overall length of capsule!      -->
+  <!-- xyz:    Position of the center of the capsule/cylinder      -->
+  <!-- direction: One of { x, y, z, -x, -y, -z }                   -->
+  <!-- =========================================================== -->
+  <xacro:macro name="collision_capsule" params="radius length xyz:='0 0 0' direction:='z'">
+    <xacro:property name="r" value="${pi/2.0 if 'y' in direction else 0}" />
+    <xacro:property name="p" value="${pi/2.0 if 'x' in direction else 0}" />
+    <xacro:property name="y" value="0" />
+    <xacro:property name="x" value="${xyz.split(' ')[0]}" />
+    <xacro:property name="y" value="${xyz.split(' ')[1]}" />
+    <xacro:property name="z" value="${xyz.split(' ')[2]}" />
+    <xacro:property name="sx" value="${length / 2.0 if 'x' in direction else 0}" />
+    <xacro:property name="sy" value="${length / 2.0 if 'y' in direction else 0}" />
+    <xacro:property name="sz" value="${length / 2.0 if 'z' in direction else 0}" />
+
+    <collision>
+      <origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${y}"/>
+      <geometry>
+        <cylinder radius="${radius}" length="${length}" />
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="${x+sx} ${y+sy} ${z+sz}" />
+      <geometry>
+        <sphere radius="${radius}" />
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="${x-sx} ${y-sy} ${z-sz}" />
+      <geometry>
+        <sphere radius="${radius}" />
+      </geometry>
+    </collision>
+  </xacro:macro>
+
   <!-- ========================================================== -->
   <!-- Adds the required tags to simulate one joint in gazebo     -->
   <!--                                                            -->

From 7f3a84f4afd061b4ace95e32a3d169a90760e0d9 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Tue, 12 Apr 2022 16:44:23 +0200
Subject: [PATCH 08/19] CHANGE: Use new `<xacro:collision_capsule>` macro for
 self collision geometries

---
 franka_description/robots/panda_arm.xacro | 192 ++++------------------
 1 file changed, 28 insertions(+), 164 deletions(-)

diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 4a42bf4a..bbbf3e48 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -10,50 +10,19 @@
         <origin rpy="${rpy}" xyz="${xyz}"/>
       </joint>
     </xacro:unless>
+
     <xacro:link_detailed_coarse name="link0">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0" />
-          <geometry>
-            <cylinder radius="${0.06+safety_distance}" length="0.03" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="-0.06 0 0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="-0.09 0 0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="-0.075 0 0.06" direction="x" radius="${0.06+safety_distance}" length="0.03" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <xacro:link_detailed_coarse name="link1">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 -0.1915" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.06+safety_distance}" length="0.2830" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.333" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.05" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 -191.5e-3" radius="${0.06+safety_distance}" length="0.283" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint1" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
       <origin rpy="0 0 0" xyz="0 0 0.333" />
@@ -63,28 +32,13 @@
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750" />
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
+
     <xacro:link_detailed_coarse name="link2">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.06+safety_distance}" length="0.12" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint2" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628" />
       <origin rpy="${-pi/2} 0 0" xyz="0 0 0" />
@@ -94,28 +48,13 @@
       <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750" />
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
+
     <xacro:link_detailed_coarse name="link3">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 -0.145" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.06+safety_distance}" length="0.15" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.22" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.07" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 -0.145" radius="${0.06+safety_distance}" length="0.15" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint3" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
       <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0" />
@@ -125,28 +64,13 @@
       <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750" />
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
+
     <xacro:link_detailed_coarse name="link4">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.06+safety_distance}" length="0.12" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-      </self_collision_geometries>
+        <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
+     </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint4" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698" />
       <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0" />
@@ -156,46 +80,14 @@
       <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750" />
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
+
     <xacro:link_detailed_coarse name="link5">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 -0.26" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.06+safety_distance}" length="0.1" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.31" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.21" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.06+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0.08 -0.13" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.025+safety_distance}" length="0.14" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0.08 -0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.025+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0.08 -0.20" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.025+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 -0.26"    radius="${0.060+safety_distance}" length="0.10" />
+        <xacro:collision_capsule xyz="0 0.08 -0.13" radius="${0.025+safety_distance}" length="0.14" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint5" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
       <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0" />
@@ -205,28 +97,13 @@
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100" />
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
+
     <xacro:link_detailed_coarse name="link6">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 -0.03" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.05+safety_distance}" length="0.08" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 0.01" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.05+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.07" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.05+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 -0.03" radius="${0.05+safety_distance}" length="0.08" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint6" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525" />
       <origin rpy="${pi/2} 0 0" xyz="0 0 0" />
@@ -236,28 +113,13 @@
       <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100" />
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
-    <xacro:link_detailed_coarse name="link7">
+
+    <xacro:link_detailed_coarse name="link7" rpy="0 0 ${pi/4}">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 0.01" rpy="0 0 0" />
-          <geometry>
-            <cylinder radius="${0.04+safety_distance}" length="0.14" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 0.08" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.04+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 -0.06" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.04+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 0.01" direction="z" radius="${0.04+safety_distance}" length="0.14" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <joint name="${arm_id}_joint7" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
       <origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
@@ -267,6 +129,7 @@
       <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
     </joint>
+
     <link name="${arm_id}_link8"/>
     <link name="${arm_id}_link8_sc">
       <collision>
@@ -292,6 +155,7 @@
       <parent link="${arm_id}_link8" />
       <child link="${arm_id}_link8_sc" />
     </joint>
+
     <joint name="${arm_id}_joint8" type="fixed">
       <origin rpy="0 0 0" xyz="0 0 0.107"/>
       <parent link="${arm_id}_link7"/>

From 848904caafe8128aabf5402b32a4009668e21528 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Tue, 12 Apr 2022 16:44:43 +0200
Subject: [PATCH 09/19] CHANGE: Move panda_link8_sc -> panda_link7_sc

The capsule at link8 covers the Pilot which is attached to link7. Though rigidly
connected to link7, link8 is more a "virtual" link without geometry indicating
the flange of the robot. That's why it makes more sense to also have the
collision capsule attached to link7_sc.
---
 franka_description/robots/panda_arm.xacro | 23 +++--------------------
 1 file changed, 3 insertions(+), 20 deletions(-)

diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index bbbf3e48..50f3afb0 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -117,6 +117,7 @@
     <xacro:link_detailed_coarse name="link7" rpy="0 0 ${pi/4}">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 0.01" direction="z" radius="${0.04+safety_distance}" length="0.14" />
+        <xacro:collision_capsule xyz="0.06 0 0.082" direction="x" radius="${0.03+safety_distance}"  length="0.01" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
 
@@ -131,26 +132,8 @@
     </joint>
 
     <link name="${arm_id}_link8"/>
-    <link name="${arm_id}_link8_sc">
-      <collision>
-        <origin xyz="0.0424 0.0424 -0.0250" rpy="${pi} ${pi/2} ${pi/2}"/>
-        <geometry>
-          <cylinder radius="${0.03+safety_distance}"  length="0.01" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0.0424 0.0424 -0.02" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.03+safety_distance}"  />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0.0424 0.0424 -0.03" rpy="0 0 0"/>
-        <geometry>
-          <sphere radius="${0.03+safety_distance}"  />
-        </geometry>
-      </collision>
-    </link>
+    <link name="${arm_id}_link8_sc"/>
+
     <joint name="${arm_id}_link8_sc_joint" type="fixed">
       <parent link="${arm_id}_link8" />
       <child link="${arm_id}_link8_sc" />

From fe476f61eb72be43ee8c5b930675d5cce26ce5a8 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Tue, 12 Apr 2022 16:56:29 +0200
Subject: [PATCH 10/19] CHANGE: Use `<xacro:collision_capsule>` also for
 hand.xacro

---
 franka_description/robots/hand.xacro | 40 +++-------------------------
 1 file changed, 4 insertions(+), 36 deletions(-)

diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 5feea387..aea03a3e 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -9,46 +9,14 @@
         <origin xyz="${xyz}" rpy="${rpy}" />
       </joint>
     </xacro:unless>
+
     <xacro:link_detailed_coarse name="hand">
       <self_collision_geometries>
-        <collision>
-          <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}" />
-          <geometry>
-            <cylinder radius="${0.04+safety_distance}" length="0.1" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 -0.05 0.04" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.04+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0.05 0.04" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.04+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}" />
-          <geometry>
-            <cylinder radius="${0.02+safety_distance}" length="0.1" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 -0.05 0.1" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.02+safety_distance}" />
-          </geometry>
-        </collision>
-        <collision>
-          <origin xyz="0 0.05 0.1" rpy="0 0 0" />
-          <geometry>
-            <sphere radius="${0.02+safety_distance}" />
-          </geometry>
-        </collision>
+        <xacro:collision_capsule xyz="0 0 0.04" direction="y" radius="${0.04+safety_distance}" length="0.1" />
+        <xacro:collision_capsule xyz="0 0 0.10" direction="y" radius="${0.02+safety_distance}" length="0.1" />
       </self_collision_geometries>
     </xacro:link_detailed_coarse>
+
     <!-- Define the hand_tcp frame -->
     <link name="${arm_id}_hand_tcp" />
     <joint name="${arm_id}_hand_tcp_joint" type="fixed">

From f0995d3c9b805765cfe1235fcb15cc9e67ebf68e Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Wed, 13 Apr 2022 10:01:43 +0200
Subject: [PATCH 11/19] ADD: Comment explaining sx,sy,sz properties in
 utils.xacro

---
 franka_description/robots/utils.xacro | 1 +
 1 file changed, 1 insertion(+)

diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index 7ba40aea..b6b92346 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -78,6 +78,7 @@
     <xacro:property name="x" value="${xyz.split(' ')[0]}" />
     <xacro:property name="y" value="${xyz.split(' ')[1]}" />
     <xacro:property name="z" value="${xyz.split(' ')[2]}" />
+    <!-- Sphere center offsets from center of cylinder -->
     <xacro:property name="sx" value="${length / 2.0 if 'x' in direction else 0}" />
     <xacro:property name="sy" value="${length / 2.0 if 'y' in direction else 0}" />
     <xacro:property name="sz" value="${length / 2.0 if 'z' in direction else 0}" />

From 9fdb0801c31712b2ab08fbf3c2ddb75386768197 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Wed, 13 Apr 2022 16:40:48 +0200
Subject: [PATCH 12/19] ADD: URDF test suite

---
 .gitignore                        |   1 +
 franka_description/CMakeLists.txt |   2 +
 franka_description/package.xml    |   1 +
 franka_description/test/urdf.py   | 200 ++++++++++++++++++++++++++++++
 4 files changed, 204 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 franka_description/test/urdf.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 00000000..0d20b648
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+*.pyc
diff --git a/franka_description/CMakeLists.txt b/franka_description/CMakeLists.txt
index f0de05cb..e64e74b6 100644
--- a/franka_description/CMakeLists.txt
+++ b/franka_description/CMakeLists.txt
@@ -10,3 +10,5 @@ install(DIRECTORY meshes
 install(DIRECTORY robots
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+catkin_add_nosetests(test/urdf.py)
diff --git a/franka_description/package.xml b/franka_description/package.xml
index a7bbc0ad..73e5010c 100644
--- a/franka_description/package.xml
+++ b/franka_description/package.xml
@@ -14,4 +14,5 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <exec_depend>xacro</exec_depend>
+  <test_depend>rosunit</test_depend>
 </package>
diff --git a/franka_description/test/urdf.py b/franka_description/test/urdf.py
new file mode 100644
index 00000000..a322c24a
--- /dev/null
+++ b/franka_description/test/urdf.py
@@ -0,0 +1,200 @@
+import sys
+import subprocess
+from unittest import TestCase
+from urdf_parser_py.urdf import URDF, Mesh, Cylinder, Sphere
+
+PKG = 'franka_description'
+
+def xacro(args=''):
+    return URDF.from_xml_string(subprocess.check_output(
+        'xacro $(rospack find %s)/robots/panda_arm.urdf.xacro %s' % (PKG, args),
+        shell=True)
+    )
+
+class TestURDF(TestCase):
+
+    def test_generate_urdf_without_xacro_args_contains_link0_up_to_link8(self):
+        urdf = xacro()
+        for i in range(0, 9):
+            link = 'panda_link%s' % i
+            self.assertTrue(link in urdf.link_map, 'Link "%s" not found in URDF' % link)
+
+    def test_generate_urdf_without_xacro_args_contains_joint1_up_to_joint8(self):
+        urdf = xacro()
+        for i in range(1, 9):
+            joint = 'panda_joint%s' % i
+            self.assertTrue(joint in urdf.joint_map, 'Joint "%s" not found in URDF' % joint)
+
+    def test_generate_urdf_without_xacro_args_dont_use_gripper(self):
+        urdf = xacro()
+        self.assertFalse(
+            filter(lambda link: 'hand' in link, urdf.link_map.keys()),
+            'Found one or more links containing "hand", probably URDF contains a franka_gripper'
+        )
+        self.assertFalse(
+            filter(lambda joint: 'hand' in joint, urdf.joint_map.keys()),
+            'Found one or more joints containing "hand", probably URDF contains a franka_gripper'
+        )
+
+    def test_generate_urdf_without_xacro_args_uses_fine_collision_models(self):
+        urdf = xacro()
+        for i in range(0,8):
+            link = urdf.link_map['panda_link%s' % i]
+            self.assertGreaterEqual(len(link.collisions), 1, "Link '%s' does not have any collision meshes assigned to it" % link.name)
+            geometry = link.collisions[0].geometry
+            self.assertIsInstance(
+                geometry, Mesh,
+                "Link '%s' is expected to have mesh geometries in <collision> not '%s'" % (link.name, geometry.__class__.__name__)
+            )
+
+    def test_generate_urdf_without_xacro_args_uses_coarse_collision_models_for_sc_links(self):
+        urdf = xacro()
+        for name in urdf.link_map:
+            if not name.endswith('_sc'): continue
+            link = urdf.link_map[name]
+            geometries = [collision.geometry for collision in link.collisions]
+            for geometry in geometries:
+                self.assertIsInstance(
+                    geometry, (Cylinder, Sphere),
+                    "Link '%s' is expected to define only a capsule <collision> geometry (made from Cylinders and Spheres, not '%s')" % (name, geometry.__class__.__name__)
+                )
+
+    def test_custom_arm_id_renames_links(self):
+        arm_id = 'foo'
+        urdf = xacro('arm_id:=%s' % arm_id)
+        for link in urdf.link_map.keys():
+            self.assertIn(arm_id, link)
+
+    def test_custom_arm_id_renames_joints(self):
+        arm_id = 'foo'
+        urdf = xacro('arm_id:=%s' % arm_id)
+        for joint in urdf.joint_map.keys():
+            self.assertIn(arm_id, joint)
+
+    def test_generate_urdf_with_hand_puts_franka_gripper_into_urdf(self):
+        arm_id = 'panda'
+        urdf = xacro('hand:=true')
+        for name in ['hand', 'hand_tcp', 'leftfinger', 'rightfinger']:
+            link = '%s_%s' % (arm_id, name)
+            self.assertIn(link, urdf.link_map)
+
+        for name in ['hand_joint', 'hand_tcp_joint', 'finger_joint1', 'finger_joint2']:
+            joint = '%s_%s' % (arm_id, name)
+            self.assertIn(joint, urdf.joint_map)
+
+    def test_custom_arm_id_with_hand_renames_hand_joints_and_links(self):
+        arm_id = 'foo'
+        urdf = xacro('arm_id:=%s hand:=true' % arm_id)
+        for name in ['hand', 'hand_tcp', 'leftfinger', 'rightfinger']:
+            link = '%s_%s' % (arm_id, name)
+            self.assertIn(link, urdf.link_map)
+
+        for name in ['hand_joint', 'hand_tcp_joint', 'finger_joint1', 'finger_joint2']:
+            joint = '%s_%s' % (arm_id, name)
+            self.assertIn(joint, urdf.joint_map)
+
+    def test_gazebo_arg_will_add_top_level_world_link(self):
+        urdf = xacro('gazebo:=true')
+
+        # Check if the world link exists
+        self.assertEqual('world', urdf.get_root())
+
+        # Check if robot is directly connected to the world link
+        self.assertListEqual(['world'], [joint.parent for joint in urdf.joints if joint.child == 'panda_link0'])
+
+    def test_gazebo_arg_will_insert_gazebo_ros_control_plugin(self):
+        urdf = xacro('gazebo:=true')
+
+        for gazebo in urdf.gazebos:
+            for child in gazebo:
+                if child.tag == 'plugin':
+                    self.assertIn('name', child.attrib, '<plugin> tag does not have a "name" attribute')
+                    self.assertEqual('gazebo_ros_control', child.attrib['name'])
+                    # Successfully found plugin, break out of all loops
+                    break
+            else:
+                # Inner loop not broken, continue with next gazebo tag
+                continue
+            # Inner loop was broken, break also outer loop
+            break
+        else:
+            # Outer loop not broken -> no valid plugin found!
+            self.fail('No <plugin name="gazebo_ros_control"> found in URDF')
+
+
+    def assert_transmissions_with(self, urdf, joints, transmission_type):
+        for name in joints:
+            for transmission in urdf.transmissions:
+                joint = transmission.joints[0]
+                interface = 'hardware_interface/' + transmission_type
+                if joint.name == name and joint.hardwareInterfaces[0] == interface:
+                    # Successfully found matching transmission, break out of loop
+                    break
+            else:
+                # Transmission Loop not broken -> no suitable transmission for joint found
+                self.fail('No suitable %s transmission tag for "%s" found in URDF' % (transmission_type, name))
+
+    def test_gazebo_arg_will_insert_position_interfaces(self):
+        urdf = xacro('gazebo:=true')
+        joints = ['panda_joint%s' % i for i in range(1,8)]
+        self.assert_transmissions_with(urdf, joints, 'PositionJointInterface')
+
+    def test_gazebo_arg_will_insert_velocity_interfaces(self):
+        urdf = xacro('gazebo:=true')
+        joints = ['panda_joint%s' % i for i in range(1,8)]
+        self.assert_transmissions_with(urdf, joints, 'VelocityJointInterface')
+
+    def test_gazebo_arg_will_insert_effort_interfaces(self):
+        urdf = xacro('gazebo:=true')
+        joints = ['panda_joint%s' % i for i in range(1,8)]
+        self.assert_transmissions_with(urdf, joints, 'EffortJointInterface')
+
+    def test_gazebo_arg_will_insert_franka_state_interface(self):
+        urdf = xacro('gazebo:=true')
+        for transmission in urdf.transmissions:
+            if transmission.type == 'franka_hw/FrankaStateInterface':
+                self.assertListEqual(
+                    ['panda_joint%s' % i for i in range(1,8)],
+                    [joint.name for joint in transmission.joints]
+                )
+                break
+        else:
+            # Didn't break out of loop -> no state interface found
+            self.fail('No franka_hw/FrankaStateInterface found in URDF')
+
+    def test_gazebo_arg_will_insert_franka_model_interface(self):
+        urdf = xacro('gazebo:=true')
+        for transmission in urdf.transmissions:
+            if transmission.type == 'franka_hw/FrankaModelInterface':
+                self.assertEqual(
+                    'panda_joint1', transmission.joints[0].name,
+                    "First joint in the FrankaModelInterface transmission must be the root of the chain, i.e. '$(arm_id)_joint1' not '%s'" % transmission.joints[0].name
+                )
+                self.assertEqual(
+                    'panda_joint8', transmission.joints[1].name,
+                    "Second joint in the FrankaModelInterface transmission must be the tip of the chain, i.e. '$(arm_id)_joint8' not '%s'" % transmission.joints[1].name
+                )
+                break
+        else:
+            # Didn't break out of loop -> no model interface found
+            self.fail('No franka_hw/FrankaModelInterface found in URDF')
+
+    def test_gazebo_arg_and_hand_will_insert_effort_interfaces_for_fingers(self):
+        urdf = xacro('gazebo:=true hand:=true')
+        joints = ['panda_finger_joint1', 'panda_finger_joint2']
+        self.assert_transmissions_with(urdf, joints, 'EffortJointInterface')
+
+    def test_setting_gazebo_arg_forces_to_have_no_geometries_inside_sc_links(self):
+        urdf = xacro('gazebo:=true')
+        for name in urdf.link_map:
+            if not name.endswith('_sc'): continue
+            link = urdf.link_map[name]
+            self.assertEqual(
+                len(link.collisions), 0,
+                "Link '%s' is expected to have no <collision> tags defined but has %s" % (name, len(link.collisions))
+            )
+
+
+if __name__ == '__main__':
+    import rosunit
+    rosunit.unitrun(PKG, 'URDF', TestURDF)

From 4b23644af26ae44c3348ed6f9b39799108ae347b Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Wed, 13 Apr 2022 18:37:20 +0200
Subject: [PATCH 13/19] ADD: urdfdom-py as CI dependency

---
 .ci/Dockerfile.melodic | 1 +
 .ci/Dockerfile.noetic  | 1 +
 2 files changed, 2 insertions(+)

diff --git a/.ci/Dockerfile.melodic b/.ci/Dockerfile.melodic
index 043c9bbc..a6e4fc47 100644
--- a/.ci/Dockerfile.melodic
+++ b/.ci/Dockerfile.melodic
@@ -12,4 +12,5 @@ RUN apt-get update -y && apt-get install -y \
     ros-melodic-gazebo-dev \
     ros-melodic-gazebo-ros-control \
     ros-melodic-orocos-kdl \
+    ros-melodic-urdfdom-py \
     && rm -rf /var/lib/apt/lists/*
diff --git a/.ci/Dockerfile.noetic b/.ci/Dockerfile.noetic
index 3bed7b2d..f5763444 100644
--- a/.ci/Dockerfile.noetic
+++ b/.ci/Dockerfile.noetic
@@ -12,4 +12,5 @@ RUN apt-get update -y && apt-get install -y \
     ros-noetic-eigen-conversions \
     ros-noetic-gazebo-dev \
     ros-noetic-gazebo-ros-control \
+    ros-noetic-urdfdom-py \
     && rm -rf /var/lib/apt/lists/*

From 46e2895dbf0257efd4f3f456a5dc1b3932a1d549 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Thu, 14 Apr 2022 09:19:04 +0200
Subject: [PATCH 14/19] FIX: URDF tests in Python3/Noetic

---
 franka_description/test/urdf.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/franka_description/test/urdf.py b/franka_description/test/urdf.py
index a322c24a..ba199810 100644
--- a/franka_description/test/urdf.py
+++ b/franka_description/test/urdf.py
@@ -28,11 +28,11 @@ def test_generate_urdf_without_xacro_args_contains_joint1_up_to_joint8(self):
     def test_generate_urdf_without_xacro_args_dont_use_gripper(self):
         urdf = xacro()
         self.assertFalse(
-            filter(lambda link: 'hand' in link, urdf.link_map.keys()),
+            list(filter(lambda link: 'hand' in link, urdf.link_map.keys())),
             'Found one or more links containing "hand", probably URDF contains a franka_gripper'
         )
         self.assertFalse(
-            filter(lambda joint: 'hand' in joint, urdf.joint_map.keys()),
+            list(filter(lambda joint: 'hand' in joint, urdf.joint_map.keys())),
             'Found one or more joints containing "hand", probably URDF contains a franka_gripper'
         )
 

From 2a4542cd619942667e59e4a477a9c4137ba3617d Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Thu, 14 Apr 2022 13:51:37 +0200
Subject: [PATCH 15/19] HACK: Disable gripper_sim_tests_with_object because
 flaky

---
 franka_gazebo/test/franka_gripper_sim_test_with_object.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/franka_gazebo/test/franka_gripper_sim_test_with_object.cpp b/franka_gazebo/test/franka_gripper_sim_test_with_object.cpp
index cffce5a2..4cfc0b90 100644
--- a/franka_gazebo/test/franka_gripper_sim_test_with_object.cpp
+++ b/franka_gazebo/test/franka_gripper_sim_test_with_object.cpp
@@ -127,7 +127,7 @@ TEST_P(GripperFailGraspFixtureTest, CanFailGrasp) {  // NOLINT(cert-err58-cpp)
 }
 
 INSTANTIATE_TEST_CASE_P(
-    GripperFailGraspFixtureTest,  // NOLINT(cert-err58-cpp)
+    DISABLED_GripperFailGraspFixtureTest,  // NOLINT(cert-err58-cpp)
     GripperFailGraspFixtureTest,
     ::testing::Combine(::testing::Values(std::make_tuple(0.04, 0.1, 0., 0.005, 0.005, 0.1),
                                          std::make_tuple(0.02, 0.1, 2., 0.01, 0.01, 0.1),
@@ -135,7 +135,7 @@ INSTANTIATE_TEST_CASE_P(
                        ::testing::Values(0, 0.1)));
 
 INSTANTIATE_TEST_CASE_P(
-    GripperGraspFixtureTest,  // NOLINT(cert-err58-cpp)
+    DISABLED_GripperGraspFixtureTest,  // NOLINT(cert-err58-cpp)
     GripperGraspFixtureTest,
     ::testing::Combine(::testing::Values(std::make_tuple(0.032, 0.1, 100., 0.01, 0.01, 0.1),
                                          std::make_tuple(0.03, 0.1, 30., 0.005, 0.005, 0.1),

From 1ac7f18f7a643338deb75670fcde8d9b8209c676 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Thu, 14 Apr 2022 15:22:56 +0200
Subject: [PATCH 16/19] ADD: Link to example of `<xacro:inertial_props>`

---
 franka_description/robots/utils.xacro | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index b6b92346..a08c1715 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -1,11 +1,11 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <!-- ============================================================ -->
-  <!-- Macro to add an <interial> tag based on yaml-load properties -->
-  <!--                                                              -->
-  <!-- name:     Name of the robot link (without prefix)            -->
-  <!-- inertial: Dictionary of inertia properties                   -->
-  <!-- ============================================================ -->
+  <!-- ============================================================== -->
+  <!-- Macro to add an <interial> tag based on yaml-load properties   -->
+  <!--                                                                -->
+  <!-- name:     Name of the robot link (without prefix)              -->
+  <!-- inertial: Dictionary of inertia properties (see inertial.yaml) -->
+  <!-- ============================================================== -->
   <xacro:macro name="inertial_props" params="inertial:=^ name:=^">
     <xacro:if value="$(arg gazebo)">
       <xacro:unless value="${name in inertial}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>

From 04f02686e5db9f3bdcaa89776c06c8ab4fe852d0 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Thu, 14 Apr 2022 15:26:22 +0200
Subject: [PATCH 17/19] CHANGE: Rename xacro:{link_detailed_coarse ->
 link_with_sc}

The name "detailed_coarse" is a bit contradicting. More suitable might be a name
which indicates that existing `<link>`s are augmented with additional
functionality such as self-collision geometries. This in mind it also should
also be rather consise so I went with `link_with_sc`.
---
 franka_description/robots/hand.xacro      |  4 +--
 franka_description/robots/panda_arm.xacro | 32 +++++++++++------------
 franka_description/robots/utils.xacro     |  3 ++-
 3 files changed, 20 insertions(+), 19 deletions(-)

diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index aea03a3e..2ca2964b 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -10,12 +10,12 @@
       </joint>
     </xacro:unless>
 
-    <xacro:link_detailed_coarse name="hand">
+    <xacro:link_with_sc name="hand">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 0.04" direction="y" radius="${0.04+safety_distance}" length="0.1" />
         <xacro:collision_capsule xyz="0 0 0.10" direction="y" radius="${0.02+safety_distance}" length="0.1" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <!-- Define the hand_tcp frame -->
     <link name="${arm_id}_hand_tcp" />
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 50f3afb0..93c4b7c2 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -11,17 +11,17 @@
       </joint>
     </xacro:unless>
 
-    <xacro:link_detailed_coarse name="link0">
+    <xacro:link_with_sc name="link0">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="-0.075 0 0.06" direction="x" radius="${0.06+safety_distance}" length="0.03" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
-    <xacro:link_detailed_coarse name="link1">
+    <xacro:link_with_sc name="link1">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 -191.5e-3" radius="${0.06+safety_distance}" length="0.283" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint1" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
@@ -33,11 +33,11 @@
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
 
-    <xacro:link_detailed_coarse name="link2">
+    <xacro:link_with_sc name="link2">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint2" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628" />
@@ -49,11 +49,11 @@
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
 
-    <xacro:link_detailed_coarse name="link3">
+    <xacro:link_with_sc name="link3">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 -0.145" radius="${0.06+safety_distance}" length="0.15" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint3" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
@@ -65,11 +65,11 @@
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
 
-    <xacro:link_detailed_coarse name="link4">
+    <xacro:link_with_sc name="link4">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
      </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint4" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698" />
@@ -81,12 +81,12 @@
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
 
-    <xacro:link_detailed_coarse name="link5">
+    <xacro:link_with_sc name="link5">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 -0.26"    radius="${0.060+safety_distance}" length="0.10" />
         <xacro:collision_capsule xyz="0 0.08 -0.13" radius="${0.025+safety_distance}" length="0.14" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint5" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
@@ -98,11 +98,11 @@
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
 
-    <xacro:link_detailed_coarse name="link6">
+    <xacro:link_with_sc name="link6">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 -0.03" radius="${0.05+safety_distance}" length="0.08" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint6" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525" />
@@ -114,12 +114,12 @@
       <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
     </joint>
 
-    <xacro:link_detailed_coarse name="link7" rpy="0 0 ${pi/4}">
+    <xacro:link_with_sc name="link7" rpy="0 0 ${pi/4}">
       <self_collision_geometries>
         <xacro:collision_capsule xyz="0 0 0.01" direction="z" radius="${0.04+safety_distance}" length="0.14" />
         <xacro:collision_capsule xyz="0.06 0 0.082" direction="x" radius="${0.03+safety_distance}"  length="0.01" />
       </self_collision_geometries>
-    </xacro:link_detailed_coarse>
+    </xacro:link_with_sc>
 
     <joint name="${arm_id}_joint7" type="revolute">
       <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro
index a08c1715..7040ee80 100644
--- a/franka_description/robots/utils.xacro
+++ b/franka_description/robots/utils.xacro
@@ -26,12 +26,13 @@
   <!-- Macro to add a single link, both with                      -->
   <!--  * detailed meshes for environmental collision checking    -->
   <!--  * and coarse geometry models for self-collision checking  -->
+  <!--    (only if 'gazebo' arg is set)                           -->
   <!--                                                            -->
   <!-- name:  Name of the robot link (without prefix)             -->
   <!-- rpy:   Rotation of the *_sc link relative to parent [rad]  -->
   <!-- self_collision_geometries:  self <collision> models        -->
   <!-- ========================================================== -->
-  <xacro:macro name="link_detailed_coarse" params="name prefix=${arm_id}_ rpy:='0 0 0' **self_collision_geometries">
+  <xacro:macro name="link_with_sc" params="name prefix=${arm_id}_ rpy:='0 0 0' **self_collision_geometries">
     <!-- sub-link defining detailed meshes for collision with environment -->
     <link name="${prefix}${name}">
       <visual>

From 034e2846a1dc2087b297e7a78d3f4f0efa664463 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 21 Apr 2022 19:54:09 +0200
Subject: [PATCH 18/19] Drop obsolete link8_sc

---
 franka_description/robots/panda_arm.xacro | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 93c4b7c2..b10c4292 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -132,12 +132,6 @@
     </joint>
 
     <link name="${arm_id}_link8"/>
-    <link name="${arm_id}_link8_sc"/>
-
-    <joint name="${arm_id}_link8_sc_joint" type="fixed">
-      <parent link="${arm_id}_link8" />
-      <child link="${arm_id}_link8_sc" />
-    </joint>
 
     <joint name="${arm_id}_joint8" type="fixed">
       <origin rpy="0 0 0" xyz="0 0 0.107"/>

From c35a2a1aa1b491c3b04255393745184f2ad43ab3 Mon Sep 17 00:00:00 2001
From: Thore Goll <thore.goll@franka.de>
Date: Fri, 22 Apr 2022 09:03:53 +0200
Subject: [PATCH 19/19] ADD: CHANGELOG entry

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1f7aebcb..43a0d374 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,6 +5,7 @@
   * `franka_example_controllers`: Extend the `teleop_joint_pd_example_controller` with a finite state machine that aligns the follower robot before starting to track the leader.
   * `franka_example_controllers`: Extend the `teleop_joint_pd_example_controller` with joint walls to actively avoid position or velocity limit violations.
   * `franka_control`: Configurable `arm_id` in launch & config files
+  * `franka_description`: URDF now contains `$(arm_id)_linkN_sc` links containing the capsule collision modules used for self-collision avoidance (MoveIt).
 
 ## 0.9.0 - 2022-03-29