Provide detailed and coarse collision models

.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/*

.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/*

.gitignore

@@ -0,0 +1 @@
+*.pyc

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
 

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)

franka_description/package.xml

@@ -14,4 +14,5 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <exec_depend>xacro</exec_depend>
+  <test_depend>rosunit</test_depend>
 </package>

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>

franka_description/robots/hand.xacro

@@ -1,93 +1,118 @@
 <?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}"/>
-        <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>
-    <link name="${arm_id}_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_with_sc>
+
+    <!-- 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://franka_description/meshes/visual/hand.dae"/>
+          <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_props 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>
-    </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://franka_description/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://franka_description/meshes/visual/finger.dae"/>
+          <box size="17.5e-3 15.2e-3 18.5e-3" />
         </geometry>
-      </visual>
-   </link>
+      </collision>
+      <xacro:inertial_props 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>

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