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..4f827dc 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>
-    <robotBaseFrame>base_footprint</robotBaseFrame>
+    <odometryRate>20</odometryRate>
+    <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>
+    <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..dff5905 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_footprint2_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>