From 732707883ce2b6a50e3b17c12191890b53f4749d Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Fri, 13 Aug 2021 10:56:51 +0200
Subject: [PATCH] Add realistic hand/finger collision meshes to the gazebo
 robot description

This commit replaces the hand/finger collision geometries that are used in the Gazebo
robot description with more accurate meshes. These meshes were taken from
[AndrejOrsula/panda_ign](https://github.com/AndrejOrsula/panda_ign/blob/master/panda/meshes/collision/finger.stl).
---
 CHANGELOG.md                                  |   1 +
 .../meshes/collision/finger.stl               | Bin 1684 -> 2684 bytes
 franka_description/robots/panda_gazebo.xacro  |  22 ++++--------------
 3 files changed, 5 insertions(+), 18 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 158d88e6a..826ccc012 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -2,6 +2,7 @@
 
 ## 0.x - UNRELEASED
 
+* Add realistic hand/finger collision geometries to the Gazebo robot description
 * Add `joint_state_desired` publisher to `franka_gazebo`
 * No further ROS Kinetic support, since [End-of-Life was in April 2021](http://wiki.ros.org/Distributions)
 
diff --git a/franka_description/meshes/collision/finger.stl b/franka_description/meshes/collision/finger.stl
index 3b87289fea8128bcec3e0b4d174b169124e8e444..e35b157ff5d571de3aeaca3fb1348f6bdf064655 100644
GIT binary patch
literal 2684
zcmbVNO-NNi6rPqQBxQ@JAfpVDo|G7&@7jgH2qGb|78$O524NYG$cWOrOd_RS1VO7d
zQuOEI>7y%Dv`Jb>SRe=$EviLWLH6@r=bM=`_sqMw>2B`Fz4Og?&YW|;X+M8vAb&R3
zTi2H#I90c=KX<w}m(Mgcw(M-&rT*h-cAO~Qy(7w~Yc;{^{Z}WVD{I@r&8sw?S>FL%
z#CKpr*>JHZ?rQ(@c&NJ}_}n=f^cQXhV9%F+Wx*y~bl9mV6~z^+oE{jg%>-IaPh8ay
z{I1`YEsbBu5-tMY{ko}zzXKb_!s@QcpXK3ra8E-pe_$@?cv<bOl5D3DK7D@a5whK~
zFc|0zDGy-I!(mNXGcp`*zkRNaGB193HKQ}fDKM7WtbunF-ugJ3#a^}FyW0p)AzO>H
zjF}OIK$0ONL_J)zOrduaB9iI1tu89A4Weai2=6D3hd&;!2vHBtQ04&!=fo1G9*BzL
zm7ReI@jdAhU2h_DRHz_hbPO~&TGqfj3c0qDVX<ZaD^(~B>vQVNzsSlw<XL(BmSP}Y
zTW8n1ejbxn<B-+hu9g};V_nZt9aYtrYOm&Um1HZ$jA~-YRkHr_)Rj6mJES$XE<VAm
zc@^Y3<D`=cc3Vb<82H1hB-=kL)Pbu~Q8nFTTpPEODwRz=`YwMeNOh^_&j%Ra>Q}Zt
zyLUu0crVt2hR--jjPVnTg&PM)rsN;V;O;Tb@P-(3OEpu-h5uk*##klUoHO33PITQz
zZ0$s*6Aj1Ahun*>>u|;iqfB1);Cj;?SzE%+8K@U3eD@tq?NWMblSw5P<!Scw;HY@+
z#7Vmd4TGWzDXM1KeTak|2N43pyL=7FkgI^%F$|-YTy$@Ri^^&eF0zBOMih?SA8Ym|
zM`SA_G>m0#19yM?QzHhKJHvK~t25U;xp9nA>Q+l`veZ4ek#5ez>6_Etmgo|$joBS8
z+A}>mj_G8-@;bx2Imr`s4%QQsZV-|9XI05qPJ^9KDe^?EVLj2sE~D}_$l)}@dZOze
zxk|FNCu)ZEL^lVrmt>D=Pt*+Si7s=TDEpv)y5xzPVLdT9cX3v9>TW00!^}7k_Tt-1
z?#tiJoRh<84eN<664`x{t>B`u@I)6?B?H<<gFG=7p6Dh=_NyNid15R)(RH|7CD}^-
E1)qWR3jhEB

literal 1684
zcmb7DSxA&o6h1j6l;wmCxzI9G5Y2DSOhGy`|Gjgg^&urOGRdKJv^2HcDk-}~%a9dW
zR<;ch*@78G2HE`ocp0cf0xQBcTa+FOX(5B4tn7b0_p$qWzwdnK`_4HFwr$>2QB@GI
z7FKL3wazUmC<_!+q|I_<c&yVatJhjxcKEMnHPAiX)Vr?ry64vr7u~v@d|dp3Jf1qm
z)9Okk&6##J^d;F7<5cuI%QW>{;wTTmG6Sg`Ne(v$k7@YFbSqUa`LKs196n1DiWR+k
z|8p|=w8LQNzq!cFn?iE?&wKF?W(>_%^vIbmlJ5)=fQ36Is!vvM;Z%=Ca3^0!nsvy#
zf2YxAp;#Lr$?5dYpHRhay3hFZLWR)YE^>b0EWve6exZin7Lk}WerouLCWhDd3GXMv
z53Yqf#EAgZG;n5#kE#u8i)pzshJ1Dv5I4=CTLx0exbm%p#5NMF4PZXN*+DfslAbYK
zLQO4+C#LancT=`!QS1+Zo#1RvD)YW?DUviW1M2}ajb0_D%^Pg*U35*&STsft`O4Zp
zwq{HD;M|O?j#{XxhKmF(Pf8bS$c)&*O3iKLbi=kp!>H;|t2RDKw3WyEsmvJhi?i4{
zHWIj(sN+m<@3QmKA<p6kE7jNA2E&+vQSv;lG@lHM{hZwxRzooZqwxFzLbYDi?W|&N
z+E4rNPvJm|b~UY-bk+N*yd}Z3)$RWFjjT?&<m>4BtL^^2fn2W5rI>*$0oaz>#pqgd
zWWqzn@H}u<xDvphAQw+&<U}@Rn53y;Hog@AM<D7um$Z_(nl4G(j(f)&3h)3LH03yF
zd+o&(=YzLh_7S|u#f45Y=?&&mtlscytX>}*Af4$;sLY6NSHy)L7ke|Q-iJ}xt<1>#
zkWHFPcIx*BIK#c;tjcVi>Wil>cV<`R8&@0Z*{4<<ir0Vs<_ymOGjJt<%$3j7ACI`0
zw=;#}-Nkh=+qjQ?ne2LwJ94ltnPC)W%jqr;jMHcS;o^$Jtl?UCg7STIoQe^*o$Ky)
zIs6RggW1?0K(vXAUGIwY>X>Yb>*A#GoB;aQE3D8zQnkb}$p`LT&MNC>zUX+F)n+d>
LNnK$LtPSuFJ&~Ep

diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro
index c2b6baf1c..984597546 100644
--- a/franka_description/robots/panda_gazebo.xacro
+++ b/franka_description/robots/panda_gazebo.xacro
@@ -288,9 +288,8 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="0.0 0 0.0375" rpy="0 ${pi/2} ${pi/2}"/>
         <geometry>
-          <box size="0.055 0.04 0.198"  />
+          <mesh filename="package://franka_description/meshes/collision/hand.stl" />
         </geometry>
       </collision>
       <inertial>
@@ -310,15 +309,8 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="0 0.015 0.03" rpy="0 0 0"/>
         <geometry>
-          <box size="0.03 0.005 0.047" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 0.007 0.045" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.03 0.015 0.017" />
+          <mesh filename="package://franka_description/meshes/collision/finger.stl" />
         </geometry>
       </collision>
     </link>
@@ -332,15 +324,9 @@
         </geometry>
       </visual>
       <collision>
-        <origin xyz="0 -0.015 0.03" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.03 0.005 0.047" />
-        </geometry>
-      </collision>
-      <collision>
-        <origin xyz="0 -0.007 0.045" rpy="0 0 0"/>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
         <geometry>
-          <box size="0.03 0.015 0.017" />
+          <mesh filename="package://franka_description/meshes/collision/finger.stl" />
         </geometry>
       </collision>
     </link>