diff --git a/robots/a1_description/urdf/a1.urdf b/robots/a1_description/urdf/a1.urdf
index 4afbcdd..418542e 100644
--- a/robots/a1_description/urdf/a1.urdf
+++ b/robots/a1_description/urdf/a1.urdf
@@ -1,9 +1,21 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="utf-8"?>
 <!-- =================================================================================== -->
 <!-- |    This document was autogenerated by xacro from robot.xacro                    | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
-<robot name="a1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="a1_description">
+  <!-- dynamics inertial value -->
+  <!-- trunk -->
+  <!-- <xacro:property name="trunk_mass" value="5.660"/>
+    <xacro:property name="trunk_com_x" value="0.012731"/>
+    <xacro:property name="trunk_com_y" value="0.002186"/>
+    <xacro:property name="trunk_com_z" value="0.000515"/>
+    <xacro:property name="trunk_ixx" value="0.016839930"/>
+    <xacro:property name="trunk_ixy" value="0.000083902"/>
+    <xacro:property name="trunk_ixz" value="0.000597679"/>
+    <xacro:property name="trunk_iyy" value="0.056579028"/>
+    <xacro:property name="trunk_iyz" value="0.000025134"/>
+    <xacro:property name="trunk_izz" value="0.064713601"/> -->
   <material name="black">
     <color rgba="0.0 0.0 0.0 1.0"/>
   </material>
@@ -31,6 +43,265 @@
   <material name="white">
     <color rgba="1.0 1.0 1.0 1.0"/>
   </material>
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/a1_gazebo</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <!-- Show the trajectory of trunk center. -->
+  <gazebo>
+    <plugin filename="libLinkPlot3DPlugin.so" name="3dplotTrunk">
+      <frequency>10</frequency>
+      <plot>
+        <link>base</link>
+        <pose>0 0 0 0 0 0</pose>
+        <material>Gazebo/Yellow</material>
+      </plot>
+    </plugin>
+  </gazebo>
+  <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+  <gazebo>
+    <plugin filename="libLinkPlot3DPlugin.so" name="3dplot">
+      <frequency>1000</frequency>
+      <plot>
+        <link>FR_calf</link>
+        <pose>0 0 -0.2 0 0 0</pose>
+        <material>Gazebo/Yellow</material>
+      </plot>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+      <bodyName>trunk</bodyName>
+      <topicName>/apply_force/trunk</topicName>
+    </plugin>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <gravity>true</gravity>
+    <sensor name="imu_sensor" type="imu">
+      <always_on>true</always_on>
+      <update_rate>1000</update_rate>
+      <visualize>true</visualize>
+      <topic>__default_topic__</topic>
+      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+        <topicName>trunk_imu</topicName>
+        <bodyName>imu_link</bodyName>
+        <updateRateHZ>1000.0</updateRateHZ>
+        <gaussianNoise>0.0</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <frameName>imu_link</frameName>
+      </plugin>
+      <pose>0 0 0 0 0 0</pose>
+    </sensor>
+  </gazebo>
+  <!-- Foot contacts. -->
+  <gazebo reference="FR_calf">
+    <sensor name="FR_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="FL_calf">
+    <sensor name="FL_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="RR_calf">
+    <sensor name="RR_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="RL_calf">
+    <sensor name="RL_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <!-- Visualization of Foot contacts. -->
+  <gazebo reference="FR_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>FR_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="FL_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>FL_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="RR_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>RR_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="RL_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>RL_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="base">
+    <material>Gazebo/Green</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+  <gazebo reference="trunk">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="stick_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/White</material>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/Red</material>
+  </gazebo>
+  <!-- FL leg -->
+  <gazebo reference="FL_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="FL_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FL_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="FL_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- FR leg -->
+  <gazebo reference="FR_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="FR_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FR_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="FR_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- RL leg -->
+  <gazebo reference="RL_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="RL_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="RL_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="RL_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- RR leg -->
+  <gazebo reference="RR_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="RR_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="RR_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="RR_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- <xacro:include filename="$(find a1_gazebo)/launch/stairs.urdf.xacro"/> -->
+  <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
+  <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+  <!-- <xacro:if value="$(arg DEBUG)">
+        <link name="world"/>
+        <joint name="base_static_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="base"/>
+        </joint>
+    </xacro:if>  -->
   <link name="base">
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -59,9 +330,9 @@
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.012731 0.002186 0.000515"/>
-      <mass value="4.713"/>
-      <inertia ixx="0.01683993" ixy="8.3902e-05" ixz="0.000597679" iyy="0.056579028" iyz="2.5134e-05" izz="0.064713601"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0041 -0.0005"/>
+      <mass value="6.0"/>
+      <inertia ixx="0.0158533" ixy="-3.66e-05" ixz="-6.11e-05" iyy="0.0377999" iyz="-2.75e-05" izz="0.0456542"/>
     </inertial>
   </link>
   <joint name="imu_joint" type="fixed">
@@ -90,12 +361,12 @@
     </collision>
   </link>
   <joint name="FR_hip_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0.183 -0.047 0"/>
+    <origin rpy="0 0 0" xyz="0.1805 -0.047 0"/>
     <parent link="trunk"/>
     <child link="FR_hip"/>
     <axis xyz="1 0 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
   </joint>
   <link name="FR_hip">
     <visual>
@@ -132,12 +403,12 @@
     </collision>
   </link>
   <joint name="FR_thigh_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 -0.08505 0"/>
+    <origin rpy="0 0 0" xyz="0 -0.0838 0"/>
     <parent link="FR_hip"/>
     <child link="FR_thigh"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
   </joint>
   <link name="FR_thigh">
     <visual>
@@ -164,8 +435,8 @@
     <parent link="FR_thigh"/>
     <child link="FR_calf"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
   </joint>
   <link name="FR_calf">
     <visual>
@@ -242,12 +513,12 @@
     </actuator>
   </transmission>
   <joint name="FL_hip_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0.183 0.047 0"/>
+    <origin rpy="0 0 0" xyz="0.1805 0.047 0"/>
     <parent link="trunk"/>
     <child link="FL_hip"/>
     <axis xyz="1 0 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
   </joint>
   <link name="FL_hip">
     <visual>
@@ -284,12 +555,12 @@
     </collision>
   </link>
   <joint name="FL_thigh_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0.08505 0"/>
+    <origin rpy="0 0 0" xyz="0 0.0838 0"/>
     <parent link="FL_hip"/>
     <child link="FL_thigh"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
   </joint>
   <link name="FL_thigh">
     <visual>
@@ -316,8 +587,8 @@
     <parent link="FL_thigh"/>
     <child link="FL_calf"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
   </joint>
   <link name="FL_calf">
     <visual>
@@ -394,12 +665,12 @@
     </actuator>
   </transmission>
   <joint name="RR_hip_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.183 -0.047 0"/>
+    <origin rpy="0 0 0" xyz="-0.1805 -0.047 0"/>
     <parent link="trunk"/>
     <child link="RR_hip"/>
     <axis xyz="1 0 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
   </joint>
   <link name="RR_hip">
     <visual>
@@ -436,12 +707,12 @@
     </collision>
   </link>
   <joint name="RR_thigh_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 -0.08505 0"/>
+    <origin rpy="0 0 0" xyz="0 -0.0838 0"/>
     <parent link="RR_hip"/>
     <child link="RR_thigh"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
   </joint>
   <link name="RR_thigh">
     <visual>
@@ -468,8 +739,8 @@
     <parent link="RR_thigh"/>
     <child link="RR_calf"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
   </joint>
   <link name="RR_calf">
     <visual>
@@ -546,12 +817,12 @@
     </actuator>
   </transmission>
   <joint name="RL_hip_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.183 0.047 0"/>
+    <origin rpy="0 0 0" xyz="-0.1805 0.047 0"/>
     <parent link="trunk"/>
     <child link="RL_hip"/>
     <axis xyz="1 0 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
   </joint>
   <link name="RL_hip">
     <visual>
@@ -588,12 +859,12 @@
     </collision>
   </link>
   <joint name="RL_thigh_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0.08505 0"/>
+    <origin rpy="0 0 0" xyz="0 0.0838 0"/>
     <parent link="RL_hip"/>
     <child link="RL_thigh"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
   </joint>
   <link name="RL_thigh">
     <visual>
@@ -620,8 +891,8 @@
     <parent link="RL_thigh"/>
     <child link="RL_calf"/>
     <axis xyz="0 1 0"/>
-    <dynamics damping="0" friction="0"/>
-    <limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
+    <dynamics damping="0.01" friction="0.2"/>
+    <limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
   </joint>
   <link name="RL_calf">
     <visual>
@@ -697,5 +968,6 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
 </robot>
 
diff --git a/robots/aliengo_description/xacro/aliengo.urdf b/robots/aliengo_description/xacro/aliengo.urdf
index 98ecfbb..8bc6072 100644
--- a/robots/aliengo_description/xacro/aliengo.urdf
+++ b/robots/aliengo_description/xacro/aliengo.urdf
@@ -1,9 +1,9 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="utf-8"?>
 <!-- =================================================================================== -->
 <!-- |    This document was autogenerated by xacro from robot.xacro                    | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
-<robot name="aliengo_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="aliengo_description">
   <material name="black">
     <color rgba="0.0 0.0 0.0 1.0"/>
   </material>
@@ -31,7 +31,254 @@
   <material name="white">
     <color rgba="1.0 1.0 1.0 1.0"/>
   </material>
-  <!-- <xacro:include filename="$(find aliengo_gazebo)/launch/gazebo.xacro"/> -->
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/aliengo_gazebo</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <!-- Show the trajectory of trunk center. -->
+  <gazebo>
+    <plugin filename="libLinkPlot3DPlugin.so" name="3dplot">
+      <frequency>10</frequency>
+      <plot>
+        <link>base</link>
+        <pose>0 0 0 0 0 0</pose>
+        <material>Gazebo/Yellow</material>
+      </plot>
+    </plugin>
+  </gazebo>
+  <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+  <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>100</frequency>
+            <plot>
+                <link>FL_foot</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Green</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+      <bodyName>trunk</bodyName>
+      <topicName>/apply_force/trunk</topicName>
+    </plugin>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <gravity>true</gravity>
+    <sensor name="imu_sensor" type="imu">
+      <always_on>true</always_on>
+      <update_rate>1000</update_rate>
+      <visualize>true</visualize>
+      <topic>__default_topic__</topic>
+      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+        <topicName>trunk_imu</topicName>
+        <bodyName>imu_link</bodyName>
+        <updateRateHZ>1000.0</updateRateHZ>
+        <gaussianNoise>0.0</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <frameName>imu_link</frameName>
+      </plugin>
+      <pose>0 0 0 0 0 0</pose>
+    </sensor>
+  </gazebo>
+  <!-- Foot contacts. -->
+  <gazebo reference="FR_calf">
+    <sensor name="FR_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="FL_calf">
+    <sensor name="FL_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="RR_calf">
+    <sensor name="RR_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="RL_calf">
+    <sensor name="RL_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <!-- Visualization of Foot contacts. -->
+  <gazebo reference="FR_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>FR_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="FL_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>FL_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="RR_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>RR_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="RL_foot">
+    <visual>
+      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
+        <topicName>RL_foot_contact</topicName>
+      </plugin>
+    </visual>
+  </gazebo>
+  <gazebo reference="base">
+    <material>Gazebo/Green</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+  <gazebo reference="trunk">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="stick_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/White</material>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/Red</material>
+  </gazebo>
+  <!-- FL leg -->
+  <gazebo reference="FL_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="FL_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FL_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="FL_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- FR leg -->
+  <gazebo reference="FR_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="FR_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FR_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="FR_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- RL leg -->
+  <gazebo reference="RL_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="RL_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="RL_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="RL_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <!-- RR leg -->
+  <gazebo reference="RR_hip">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="RR_thigh">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="RR_calf">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="RR_foot">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
   <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
   <!-- <xacro:if value="$(arg DEBUG)">
         <link name="world"/>