From a1efd5b6095b1fa844bbadfe65a5a723ffa4de1d Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@gmail.com>
Date: Wed, 1 Apr 2020 21:59:58 +0800
Subject: [PATCH] fixed friction issue

---
 .../launch/scout_v2_stock.launch              |  1 -
 scout_description/urdf/scout_v2.gazebo        | 20 ++++++++++++++++++-
 scout_description/urdf/scout_v2.xacro         | 16 +++++++--------
 scout_description/urdf/scout_wheel.gazebo     | 11 ----------
 scout_gazebo_sim/launch/scout_playpen.launch  |  1 -
 .../worlds/weston_robot_empty.world           | 17 +++++-----------
 6 files changed, 32 insertions(+), 34 deletions(-)

diff --git a/scout_description/launch/scout_v2_stock.launch b/scout_description/launch/scout_v2_stock.launch
index 0560131..337b624 100644
--- a/scout_description/launch/scout_v2_stock.launch
+++ b/scout_description/launch/scout_v2_stock.launch
@@ -4,7 +4,6 @@
     <arg name="urdf_extras" default="$(find scout_description)/urdf/empty.urdf"/>
 
     <param name="robot_description" command="$(find xacro)/xacro '$(find scout_description)/urdf/scout_v2.xacro'
-        --inorder 
         robot_namespace:=$(arg robot_namespace) 
         urdf_extras:=$(arg urdf_extras)" />
 </launch>
diff --git a/scout_description/urdf/scout_v2.gazebo b/scout_description/urdf/scout_v2.gazebo
index e35d56c..0cdc2b1 100644
--- a/scout_description/urdf/scout_v2.gazebo
+++ b/scout_description/urdf/scout_v2.gazebo
@@ -1,5 +1,12 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" >
+
+Reference:
+[1] https://answers.ros.org/question/246914/four-wheeled-skid-steering-in-gazebo-and-ros-using-gazebo-ros-control/
+[2] https://answers.ros.org/question/10119/gazebo-controller-for-skid-steering/
+[3] https://answers.ros.org/question/9640/rotation-error-in-gazebo-simulation/
+[4] https://www.youtube.com/watch?v=fuRAv6PDwdw
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
     <xacro:include filename="$(find scout_description)/urdf/scout_wheel.gazebo" />
 
     <!-- Additional definitions for simulation -->
@@ -20,6 +27,17 @@
         </plugin>
     </gazebo>
 
+    <!-- kp spring constant, kd damping constant -->
+    <gazebo>
+        <mu1 value="1.0"/>
+        <mu2 value="0.9"/>
+        <kp value="10000000.0" />
+        <kd value="1.0" />
+        <fdir1 value="1 0 0"/>
+        <minDepth value="0.001" />
+        <maxContacts value="64"/>
+    </gazebo>
+
     <!-- <gazebo>
         <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
             <updateRate>100.0</updateRate>
diff --git a/scout_description/urdf/scout_v2.xacro b/scout_description/urdf/scout_v2.xacro
index 462bf1a..0b68053 100644
--- a/scout_description/urdf/scout_v2.xacro
+++ b/scout_description/urdf/scout_v2.xacro
@@ -32,6 +32,14 @@
         <child link="base_link" />
     </joint>
 
+    <!-- For testing, hang the robot up in the air -->
+    <!-- <link name="world" />
+    <joint name="world_to_base_link=" type="fixed">
+        <origin xyz="0 0 0.5" rpy="0 0 0" />
+        <parent link="world"/>
+        <child link="base_link"/>
+    </joint> -->
+
     <!-- Base link -->
     <link name="base_link">
         <visual>
@@ -87,14 +95,6 @@
         <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
     </xacro:scout_wheel_type2>
 
-    <!-- For testing, hang the robot up in the air -->
-    <!-- <link name="world" />
-    <joint name="world_to_base_link=" type="fixed">
-        <origin xyz="0 0 0.5" rpy="0 0 0" />
-        <parent link="world"/>
-        <child link="base_link"/>
-    </joint> -->
-
     <!-- Additional definitions -->
     <xacro:include filename="$(arg urdf_extras)" />
 
diff --git a/scout_description/urdf/scout_wheel.gazebo b/scout_description/urdf/scout_wheel.gazebo
index c867ce2..afabecc 100644
--- a/scout_description/urdf/scout_wheel.gazebo
+++ b/scout_description/urdf/scout_wheel.gazebo
@@ -28,17 +28,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 <robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
 
     <xacro:macro name="scout_wheel_transmission" params="wheel_prefix">
-        <!-- kp spring constant, kd damping constant -->
-        <gazebo reference="${wheel_prefix}_wheel_link">
-            <mu1 value="1.0"/>
-            <mu2 value="0.9"/>
-            <kp value="10000000.0" />
-            <kd value="1.0" />
-            <fdir1 value="1 0 0"/> 
-            <minDepth value="0.001" />
-            <maxContacts value="64"/>
-        </gazebo> 
-
         <transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
             <type>transmission_interface/SimpleTransmission</type>
             <actuator name="${wheel_prefix}_wheel_motor">
diff --git a/scout_gazebo_sim/launch/scout_playpen.launch b/scout_gazebo_sim/launch/scout_playpen.launch
index afefa40..00ede1d 100644
--- a/scout_gazebo_sim/launch/scout_playpen.launch
+++ b/scout_gazebo_sim/launch/scout_playpen.launch
@@ -29,7 +29,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
     <arg name="world_name" default="$(find scout_gazebo_sim)/worlds/clearpath_playpen.world"/>
 
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <arg name="physics" value="bullet" />
         <arg name="world_name" value="$(arg world_name)"/>
         <arg name="paused" value="false"/>
         <arg name="use_sim_time" value="true"/>
diff --git a/scout_gazebo_sim/worlds/weston_robot_empty.world b/scout_gazebo_sim/worlds/weston_robot_empty.world
index bb55236..284ed8d 100644
--- a/scout_gazebo_sim/worlds/weston_robot_empty.world
+++ b/scout_gazebo_sim/worlds/weston_robot_empty.world
@@ -12,18 +12,11 @@
     </include>
 
     <!-- Physics -->
-    <!-- <physics type="ode">
-      <ode>
-        <solver>
-          <type>world</type>
-        </solver>
-        <constraints>
-          <contact_max_correcting_vel>0.1</contact_max_correcting_vel>
-          <contact_surface_layer>0.0001</contact_surface_layer>
-        </constraints>
-      </ode>
-      <max_step_size>0.001</max_step_size>
+    <physics type='ode'>
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>100</real_time_update_rate>
+      <gravity>0 0 -9.8</gravity>
     </physics>
-    <gravity>0.0 0.0 -9.81</gravity> -->
   </world>
 </sdf>
\ No newline at end of file