From 47e869a01b5bf92ca203110d31c4dbeb30b3369f Mon Sep 17 00:00:00 2001
From: Alexander Moriarty <amoriarty@fetchrobotics.com>
Date: Fri, 22 Mar 2019 11:25:01 -0700
Subject: [PATCH] fetch_gazebo: add casters to fetch.gazebo.xacro

This adds 4 casters to the gazebo model, and updates the physics params.

This should fix #31.

This is related to #37
---
 fetch_gazebo/robots/fetch.gazebo.xacro | 68 +++++++++++++++++++++-----
 1 file changed, 56 insertions(+), 12 deletions(-)

diff --git a/fetch_gazebo/robots/fetch.gazebo.xacro b/fetch_gazebo/robots/fetch.gazebo.xacro
index b826ea2..50a5022 100644
--- a/fetch_gazebo/robots/fetch.gazebo.xacro
+++ b/fetch_gazebo/robots/fetch.gazebo.xacro
@@ -1,6 +1,50 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fetch" >
   <xacro:include filename="$(find fetch_description)/robots/fetch.urdf" />
 
+  <!-- Add four casters to the base -->
+  <xacro:macro name="caster" params="prefix joint_x joint_y">
+  <link name="${prefix}_caster_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.0601"/> <!-- 0.06033 -->
+      </geometry>
+      <material name="">
+        <color rgba="0.086 0.506 0.767 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.0601"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.01"/>
+    </inertial>
+  </link>
+  <gazebo reference="${prefix}_caster_link">
+    <mu1>0.01</mu1>
+    <mu2>0.01</mu2>
+    <kp>100000000</kp>
+    <kd>10.0</kd>
+    <maxVel>10</maxVel>
+    <minDepth>0.001</minDepth>
+  </gazebo>
+  <joint name="${prefix}_caster_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="${prefix}_caster_link"/>
+    <origin rpy="0 0 0" xyz="${joint_x} ${joint_y} 0.055325"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  </xacro:macro>
+  <xacro:caster prefix="fl" joint_x="0.15" joint_y="0.12"/>
+  <xacro:caster prefix="fr" joint_x="0.15" joint_y="-0.12"/>
+  <xacro:caster prefix="br" joint_x="-0.2" joint_y="0.12"/>
+  <xacro:caster prefix="bl" joint_x="-0.2" joint_y="-0.12"/>
+
   <!-- Base is modeled as a big tub sitting on floor, with two wheels -->
   <gazebo reference="base_link">
     <kp>100000000.0</kp>
@@ -12,23 +56,23 @@
     <minDepth>0.0005</minDepth>
   </gazebo>
   <gazebo reference="r_wheel_link">
-    <kp>500000.0</kp>
-    <kd>10.0</kd>
-    <mu1>10</mu1>
-    <mu2>10</mu2>
-    <fdir1>1 0 0</fdir1>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <mu1>10.0</mu1>
+    <mu2>10.0</mu2>
+    <!-- <fdir1>1 0 0</fdir1 -->
     <maxVel>1.0</maxVel>
-    <minDepth>0.003</minDepth>
+    <minDepth>0.01</minDepth>
     <material>Gazebo/Black</material>
   </gazebo>
   <gazebo reference="l_wheel_link">
-    <kp>500000.0</kp>
-    <kd>10.0</kd>
-    <mu1>10</mu1>
-    <mu2>10</mu2>
-    <fdir1>1 0 0</fdir1>
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <mu1>10.0</mu1>
+    <mu2>10.0</mu2>
+    <!-- fdir1>1 0 0</fdir1 -->
     <maxVel>1.0</maxVel>
-    <minDepth>0.003</minDepth>
+    <minDepth>0.01</minDepth>
     <material>Gazebo/Black</material>
   </gazebo>
   <gazebo reference="l_wheel_joint">