From 208fbc6a6ba7cdba6d42f5bbe44b900d01286c36 Mon Sep 17 00:00:00 2001
From: Kanae Kochigami <kochigami@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 1 Dec 2016 12:27:04 +0900
Subject: [PATCH 1/2] [pepper_gazebo_plugin] add base_footprint2 to use planar
 move plugin

---
 .../urdf/pepper1.0_generated_urdf/pepper.urdf | 35 ++++++++++++------
 .../pepperGazebo.xacro                        | 36 +++++++++++++------
 .../pepper_torso.xacro                        | 22 ++++++++++++
 3 files changed, 73 insertions(+), 20 deletions(-)

diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf b/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf
index 3126bd9..a1ee55a 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf
@@ -2279,20 +2279,35 @@
   </sensor>
 </gazebo> 
 -->
+  <!-- <gazebo> -->
+  <!--   <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller"> -->
+  <!--     <alwaysOn>true</alwaysOn> -->
+  <!--     <updateRate>100</updateRate> -->
+  <!--     <robotNamespace>/</robotNamespace> -->
+  <!--     <leftJoint>WheelFL</leftJoint> -->
+  <!--     <rightJoint>WheelFR</rightJoint> -->
+  <!--     <wheelSeparation>0.170</wheelSeparation> -->
+  <!--     <wheelDiameter>0.140</wheelDiameter> -->
+  <!--     <torque>36.9</torque> -->
+  <!--     <commandTopic>cmd_vel</commandTopic> -->
+  <!--     <odometryTopic>odom_diffdrive</odometryTopic> -->
+  <!--     <odometryFrame>odom</odometryFrame> -->
+  <!--     <robotBaseFrame>base_footprint</robotBaseFrame> -->
+  <!--   </plugin> -->
+  <!-- </gazebo> -->
+  <!-- Planar move plugin -->
   <gazebo>
-    <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>100</updateRate>
-      <robotNamespace>/</robotNamespace>
-      <leftJoint>WheelFL</leftJoint>
-      <rightJoint>WheelFR</rightJoint>
-      <wheelSeparation>0.170</wheelSeparation>
-      <wheelDiameter>0.140</wheelDiameter>
-      <torque>36.9</torque>
+    <plugin name="object_controller"
+            filename="libgazebo_ros_planar_move.so">
       <commandTopic>cmd_vel</commandTopic>
-      <odometryTopic>odom_diffdrive</odometryTopic>
+      <odometryTopic>odom</odometryTopic>
       <odometryFrame>odom</odometryFrame>
+      <odometryRate>20</odometryRate>
       <robotBaseFrame>base_footprint</robotBaseFrame>
+      <recover_roll_velocity_p_gain>1</recover_roll_velocity_p_gain>
+      <recover_pitch_velocity_p_gain>1</recover_pitch_velocity_p_gain>
+      <recover_z_velocity_p_gain>1</recover_z_velocity_p_gain>
+      <joint_state_idel_sec>3.0</joint_state_idel_sec>
     </plugin>
   </gazebo>
   <!-- HEAD -->
diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
index 8f1a326..67e3ec7 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
@@ -684,20 +684,36 @@
 </gazebo> 
 -->
 <!-- Drive controller -->
+<!-- <gazebo> -->
+<!--   <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so"> -->
+<!--     <alwaysOn>true</alwaysOn> -->
+<!--     <updateRate>100</updateRate> -->
+<!--     <robotNamespace>/</robotNamespace> -->
+<!--     <leftJoint>WheelFL</leftJoint> -->
+<!--     <rightJoint>WheelFR</rightJoint> -->
+<!--     <wheelSeparation>0.170</wheelSeparation> -->
+<!--     <wheelDiameter>0.140</wheelDiameter> -->
+<!--     <torque>36.9</torque> -->
+<!--     <commandTopic>cmd_vel</commandTopic> -->
+<!--     <odometryTopic>odom_diffdrive</odometryTopic> -->
+<!--     <odometryFrame>odom</odometryFrame> -->
+<!--     <robotBaseFrame>base_footprint</robotBaseFrame> -->
+<!--   </plugin> -->
+<!-- </gazebo> -->
+
+<!-- Planar move plugin -->
 <gazebo>
-  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
-    <alwaysOn>true</alwaysOn>
-    <updateRate>100</updateRate>
-    <robotNamespace>/</robotNamespace>
-    <leftJoint>WheelFL</leftJoint>
-    <rightJoint>WheelFR</rightJoint>
-    <wheelSeparation>0.170</wheelSeparation>
-    <wheelDiameter>0.140</wheelDiameter>
-    <torque>36.9</torque>
+  <plugin name="object_controller"
+	  filename="libgazebo_ros_planar_move.so">
     <commandTopic>cmd_vel</commandTopic>
-    <odometryTopic>odom_diffdrive</odometryTopic>
+    <odometryTopic>odom</odometryTopic>
     <odometryFrame>odom</odometryFrame>
+    <odometryRate>20</odometryRate>
     <robotBaseFrame>base_footprint</robotBaseFrame>
+    <recover_roll_velocity_p_gain>1</recover_roll_velocity_p_gain>
+    <recover_pitch_velocity_p_gain>1</recover_pitch_velocity_p_gain>
+    <recover_z_velocity_p_gain>1</recover_z_velocity_p_gain>
+    <joint_state_idel_sec>3.0</joint_state_idel_sec>
   </plugin>
 </gazebo>
 </robot>
diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro
index aeec177..a2cb757 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro
@@ -18,4 +18,26 @@
 		</inertial>
 		<xacro:insert_visu_torso/>
 	</link>
+	 <link name="base_footprint2">
+           <visual>
+             <geometry>
+               <box size="0.01 0.01 0.01" />
+             </geometry>
+           </visual>
+           <collision>
+             <geometry>
+               <box size="1.2 1.2 0.01" />
+             </geometry>
+           </collision>
+           <inertial>
+             <mass value="1"/>
+             <inertia ixx="0.0452761" ixy="0" ixz="0.00499091" iyy="0.0432702" iyz="-0.000266886" izz="0.0258881"/>
+           </inertial>
+	 </link>
+	 <joint name="base_footprint_fixedjoint" type="fixed">
+           <parent link="base_link"/>
+           <child link="base_footprint2"/>
+	   <origin rpy="0 0 0" xyz="0 0 -0.8"/>
+           <axis xyz="0 0 0"/>
+	 </joint>
 </robot>

From 1e8bed7bde4635156b1dbd524d5a14d9c274077b Mon Sep 17 00:00:00 2001
From: Kanae Kochigami <kochigami@jsk.imi.i.u-tokyo.ac.jp>
Date: Fri, 9 Dec 2016 13:51:57 +0900
Subject: [PATCH 2/2] modify joint name

---
 .../urdf/pepper1.0_generated_urdf/pepperGazebo.xacro            | 2 +-
 .../urdf/pepper1.0_generated_urdf/pepper_torso.xacro            | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
index 67e3ec7..4f827dc 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
@@ -709,7 +709,7 @@
     <odometryTopic>odom</odometryTopic>
     <odometryFrame>odom</odometryFrame>
     <odometryRate>20</odometryRate>
-    <robotBaseFrame>base_footprint</robotBaseFrame>
+    <robotBaseFrame>base_footprint2</robotBaseFrame>
     <recover_roll_velocity_p_gain>1</recover_roll_velocity_p_gain>
     <recover_pitch_velocity_p_gain>1</recover_pitch_velocity_p_gain>
     <recover_z_velocity_p_gain>1</recover_z_velocity_p_gain>
diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro
index a2cb757..dff5905 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepper_torso.xacro
@@ -34,7 +34,7 @@
              <inertia ixx="0.0452761" ixy="0" ixz="0.00499091" iyy="0.0432702" iyz="-0.000266886" izz="0.0258881"/>
            </inertial>
 	 </link>
-	 <joint name="base_footprint_fixedjoint" type="fixed">
+	 <joint name="base_footprint2_fixedjoint" type="fixed">
            <parent link="base_link"/>
            <child link="base_footprint2"/>
 	   <origin rpy="0 0 0" xyz="0 0 -0.8"/>