From ce87ceb4d2bda9286888155377d60cdeb87fb259 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 12 Nov 2021 11:58:49 +0100
Subject: [PATCH] Adaptions for
 https://github.com/frankaemika/franka_ros/pull/199

---
 config/arm.xacro  | 73 ++++++++++++++++++++++++++++++-----------------
 config/hand.xacro | 20 +++++++++----
 2 files changed, 61 insertions(+), 32 deletions(-)

diff --git a/config/arm.xacro b/config/arm.xacro
index e89b0c5..f6827f1 100644
--- a/config/arm.xacro
+++ b/config/arm.xacro
@@ -32,30 +32,51 @@
 
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0" />
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
-    <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />
-    <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never" />
-    <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never" />
-    <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent" />
-    <disable_collisions link1="panda_link1" link2="panda_link3" reason="Default" />
-    <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never" />
-    <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent" />
-    <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never" />
-    <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent" />
-    <disable_collisions link1="panda_link3" link2="panda_link5" reason="Default" />
-    <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never" />
-    <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent" />
-    <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never" />
-    <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
-    <disable_collisions link1="panda_link4" link2="panda_link8" reason="Never" />
-    <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
-    <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
-    <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
-    <disable_collisions link1="panda_link7" link2="panda_link8" reason="Adjacent" />
-    <!-- Fix self-collisions between coarse collision geometries provided by franka_description
-            (see https://github.com/ros-planning/panda_moveit_config/pull/35#pullrequestreview-390715967).
-            These collisions are handled by joint limits already and thus can be disabled here. -->
-    <disable_collisions link1="panda_link5" link2="panda_link8" reason="Default" />
-    <disable_collisions link1="panda_link6" link2="panda_link8" reason="Default" />
+
+    <xacro:macro name="collision" params="link">
+        <!-- Enable (environmental) collisions of ${link}_sc -->
+        <collision_default link="${link}_sc" allow="ALWAYS" />
+        <!-- Disable collisions of link with any other arm link, as these are handled by the "sc" links -->
+        <disable_collisions link1="${link}" link2="panda_link0" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link1" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link2" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link3" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link4" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link5" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link6" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link7" reason="Default" />
+        <disable_collisions link1="${link}" link2="panda_link8" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link0" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link1" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link2" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link3" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link4" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link5" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link6" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link7" reason="Default" />
+        <disable_collisions link1="${link}_sc" link2="panda_link8" reason="Default" />
+    </xacro:macro>
+    <xacro:collision link="panda_link0"/>
+    <enable_collisions link1="panda_link0_sc" link2="panda_link5_sc" />
+    <enable_collisions link1="panda_link0_sc" link2="panda_link6_sc" />
+    <enable_collisions link1="panda_link0_sc" link2="panda_link7_sc" />
+    <enable_collisions link1="panda_link0_sc" link2="panda_link8_sc" />
+    <xacro:collision link="panda_link1" />
+    <enable_collisions link1="panda_link1_sc" link2="panda_link5_sc" />
+    <enable_collisions link1="panda_link1_sc" link2="panda_link6_sc" />
+    <enable_collisions link1="panda_link1_sc" link2="panda_link7_sc" />
+    <enable_collisions link1="panda_link1_sc" link2="panda_link8_sc" />
+    <xacro:collision link="panda_link2" />
+    <enable_collisions link1="panda_link2_sc" link2="panda_link5_sc" />
+    <enable_collisions link1="panda_link2_sc" link2="panda_link6_sc" />
+    <enable_collisions link1="panda_link2_sc" link2="panda_link7_sc" />
+    <enable_collisions link1="panda_link2_sc" link2="panda_link8_sc" />
+    <xacro:collision link="panda_link3" />
+    <enable_collisions link1="panda_link3_sc" link2="panda_link7_sc" />
+    <enable_collisions link1="panda_link3_sc" link2="panda_link8_sc" />
+    <xacro:collision link="panda_link4" />
+    <xacro:collision link="panda_link5" />
+    <xacro:collision link="panda_link6" />
+    <xacro:collision link="panda_link7" />
+    <xacro:collision link="panda_link8" />
 </robot>
diff --git a/config/hand.xacro b/config/hand.xacro
index abae053..3f1e608 100644
--- a/config/hand.xacro
+++ b/config/hand.xacro
@@ -13,7 +13,7 @@
         <disable_collisions link1="${finger}" link2="panda_link7" reason="Default" />
         <disable_collisions link1="${finger}" link2="panda_link8" reason="Default" />
         <disable_collisions link1="${finger}" link2="panda_hand" reason="Default" />
-        <disable_collisions link1="${finger}" link2="panda_hand_coarse" reason="Default" />
+        <disable_collisions link1="${finger}" link2="panda_hand_sc" reason="Default" />
     </xacro:macro>
     <xacro:macro name="hand">
         <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
@@ -39,11 +39,19 @@
         <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
         <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
         <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-        <collision_default link="panda_hand_coarse" allow="ALWAYS" />
-        <enable_collisions link1="panda_hand_coarse" link2="panda_link0" />
-        <enable_collisions link1="panda_hand_coarse" link2="panda_link1" />
-        <enable_collisions link1="panda_hand_coarse" link2="panda_link2" />
-        <enable_collisions link1="panda_hand_coarse" link2="panda_link3" />
+        <collision_default link="panda_hand_sc" allow="ALWAYS" />
+        <enable_collisions link1="panda_hand_sc" link2="panda_link0" />
+        <enable_collisions link1="panda_hand_sc" link2="panda_link1" />
+        <enable_collisions link1="panda_hand_sc" link2="panda_link2" />
+        <enable_collisions link1="panda_hand_sc" link2="panda_link3" />
+        <!-- Disable collision of hand with all arm links. These are handled by the *_sc links -->
+        <disable_collisions link1="panda_hand" link2="panda_link0" />
+        <disable_collisions link1="panda_hand" link2="panda_link1" />
+        <disable_collisions link1="panda_hand" link2="panda_link2" />
+        <disable_collisions link1="panda_hand" link2="panda_link3" />
+        <disable_collisions link1="panda_hand" link2="panda_link4" />
+        <disable_collisions link1="panda_hand" link2="panda_link5" />
+        <disable_collisions link1="panda_hand" link2="panda_link6" />
         <disable_collisions link1="panda_hand" link2="panda_link7" />
         <disable_collisions link1="panda_hand" link2="panda_link8" />