From 137c30860658ef6393e3b2a73658170d5133f96a Mon Sep 17 00:00:00 2001
From: Konstantinos Chatzilygeroudis <costashatz@gmail.com>
Date: Fri, 30 May 2014 02:47:20 +0300
Subject: [PATCH] Improved Gazebo a little more. Added argument to choose
 whether to include pids on the motors. Without the pids the motion is very
 good, but robot slides and in some cases the forces get extremely large.
 Also, fixed/tweaked values in URDF/xacro

---
 .../launch/nao_dcm_gazebo_H21.launch          |  8 ++++-
 .../launch/nao_dcm_gazebo_H25.launch          |  8 ++++-
 .../urdf/nao_dcm_gazebo.xacro                 | 36 +++++++++----------
 3 files changed, 32 insertions(+), 20 deletions(-)

diff --git a/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch
index 2e1a06c..a261954 100644
--- a/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch
+++ b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch
@@ -8,7 +8,13 @@
     <arg name="paused" value="true"/>
     <!-- more default parameters can be changed here -->
   </include>
-  <rosparam file="$(find nao_dcm_gazebo)/config/gazebo_ros_control_params.yaml" command="load"/>
+
+  <!-- Use PIDs?? -->
+  <arg name="use_pid" value="false"/>
+
+  <group if="$(arg use_pid)">
+    <rosparam file="$(find nao_dcm_gazebo)/config/gazebo_ros_control_params.yaml" command="load"/>
+  </group>
 
   <!-- Call Nao Robot Trajectory Controller -->
   <include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
diff --git a/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch
index 14dc745..5d31718 100644
--- a/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch
+++ b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch
@@ -8,7 +8,13 @@
     <arg name="paused" value="true"/>
     <!-- more default parameters can be changed here -->
   </include>
-  <rosparam file="$(find nao_dcm_gazebo)/config/gazebo_ros_control_params.yaml" command="load"/>
+
+  <!-- Use PIDs?? -->
+  <arg name="use_pid" value="false"/>
+
+  <group if="$(arg use_pid)">
+    <rosparam file="$(find nao_dcm_gazebo)/config/gazebo_ros_control_params.yaml" command="load"/>
+  </group>
 
   <!-- Call Nao Robot Trajectory Controller -->
   <include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
index ffadaa8..3367a37 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
@@ -141,63 +141,63 @@
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LFinger12">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LFinger13">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LFinger21">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LFinger22">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LFinger23">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LThumb1">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LThumb2">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="l_gripper">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RShoulder">
         <mu1>0.5</mu1>
@@ -230,63 +230,63 @@
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RFinger12">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RFinger13">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RFinger21">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RFinger22">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RFinger23">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RThumb1">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="RThumb2">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="r_gripper">
         <material>Gazebo/Yellow</material>
         <mu1>0.5</mu1>
         <mu2>0.5</mu2>
         <selfCollide>false</selfCollide>
-        <turnGravityOff>false</turnGravityOff>
+        <turnGravityOff>true</turnGravityOff>
     </gazebo>
     <gazebo reference="LHipYaw">
         <material>Gazebo/Green</material>