From 58f23c7f1c4345de8963fc01ee67821d645227e4 Mon Sep 17 00:00:00 2001
From: Victor Lopez <victor.lopez@pal-robotics.com>
Date: Fri, 28 May 2021 15:39:07 +0000
Subject: [PATCH] Tune a bit physics to avoid joint slippage, specially in
 TIAGo

---
 pmb2_description/urdf/wheels/caster.gazebo.xacro | 12 ++++++------
 pmb2_description/urdf/wheels/caster.urdf.xacro   |  3 ++-
 pmb2_description/urdf/wheels/wheel.urdf.xacro    |  1 +
 3 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/pmb2_description/urdf/wheels/caster.gazebo.xacro b/pmb2_description/urdf/wheels/caster.gazebo.xacro
index 1de6c4b5..30d753ca 100644
--- a/pmb2_description/urdf/wheels/caster.gazebo.xacro
+++ b/pmb2_description/urdf/wheels/caster.gazebo.xacro
@@ -16,7 +16,7 @@
 
     <gazebo reference="caster_${side}_1_link">
       <kp>100000000.0</kp>
-      <kd>10.0</kd>
+      <kd>100000.0</kd>
       <friction>
         <ode>
           <mu>1.0</mu>
@@ -24,14 +24,14 @@
         </ode>
       </friction>
       <!--<fdir1>1 0 0</fdir1>-->
-      <max_vel>10.0</max_vel>
-      <min_depth>0.0005</min_depth>
+      <max_vel>1.0</max_vel>
+      <min_depth>0.005</min_depth>
       <material>Gazebo/DarkGrey</material>
     </gazebo>
 
     <gazebo reference="caster_${side}_2_link">
       <kp>100000000.0</kp>
-      <kd>10.0</kd>
+      <kd>100000.0</kd>
       <friction>
         <ode>
           <mu>1.0</mu>
@@ -39,8 +39,8 @@
         </ode>
       </friction>
       <!--<fdir1>1 0 0</fdir1>-->
-      <max_vel>10.0</max_vel>
-      <min_depth>0.0005</min_depth>
+      <max_vel>1.0</max_vel>
+      <min_depth>0.005</min_depth>
       <material>Gazebo/DarkGrey</material>
     </gazebo>
 
diff --git a/pmb2_description/urdf/wheels/caster.urdf.xacro b/pmb2_description/urdf/wheels/caster.urdf.xacro
index 9a7a3675..bee44a3d 100644
--- a/pmb2_description/urdf/wheels/caster.urdf.xacro
+++ b/pmb2_description/urdf/wheels/caster.urdf.xacro
@@ -64,7 +64,7 @@
       <child link="caster_${side}_1_link"/>
       <origin xyz="${offset_x + separation_x / 2} ${offset_y - separation_y / 2} ${offset_z}" rpy="0 0 0"/>
       <axis xyz="0 0 1"/>
-      <dynamics damping="0.005" friction="0.0"/>
+      <dynamics damping="0.015" friction="0.0"/>
     </joint>
 
     <joint name="caster_${side}_2_joint" type="continuous">
@@ -72,6 +72,7 @@
       <child link="caster_${side}_2_link"/>
       <origin xyz="-0.016 0.0000 -0.040" rpy="${-90.0 * deg_to_rad} 0 0"/>
       <axis xyz="0 0 1"/>
+      <dynamics damping="0.015" friction="0.0"/>
     </joint>
 
     <!-- Gazebo extensions -->
diff --git a/pmb2_description/urdf/wheels/wheel.urdf.xacro b/pmb2_description/urdf/wheels/wheel.urdf.xacro
index f4b779ed..25397fe2 100644
--- a/pmb2_description/urdf/wheels/wheel.urdf.xacro
+++ b/pmb2_description/urdf/wheels/wheel.urdf.xacro
@@ -72,6 +72,7 @@ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041
           <origin xyz="0 ${-separation / 2 * reflect} 0.0" rpy="${-90.0 * deg_to_rad} 0 0"/>
           <axis xyz="0 0 1"/>
           <limit effort="${torque}" velocity="${velocity / radius}"/>
+        <dynamics damping="1.0" friction="0.0"/>
         </joint>
 
         <!-- Gazebo extensions -->