diff --git a/robots/aliengo_description/xacro/robot.xacro b/robots/aliengo_description/xacro/robot.xacro
index 2ae1f8b..3ea41d7 100644
--- a/robots/aliengo_description/xacro/robot.xacro
+++ b/robots/aliengo_description/xacro/robot.xacro
@@ -75,7 +75,7 @@
         <inertial>
             <mass value="0.001"/>
             <origin rpy="0 0 0" xyz="0 0 0"/>
-            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
         </inertial>
         <visual>
             <origin rpy="0 0 0" xyz="0 0 0"/>
diff --git a/robots/b1_description/xacro/robot.xacro b/robots/b1_description/xacro/robot.xacro
index 0f98e58..a945fd1 100644
--- a/robots/b1_description/xacro/robot.xacro
+++ b/robots/b1_description/xacro/robot.xacro
@@ -75,7 +75,7 @@
         <inertial>
             <mass value="0.001"/>
             <origin rpy="0 0 0" xyz="0 0 0"/>
-            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
         </inertial>
         <visual>
             <origin rpy="0 0 0" xyz="0 0 0"/>
diff --git a/robots/go1_description/launch/check_joint.rviz b/robots/go1_description/launch/check_joint.rviz
index 08ef857..dd0bd51 100644
--- a/robots/go1_description/launch/check_joint.rviz
+++ b/robots/go1_description/launch/check_joint.rviz
@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 796
+    Tree Height: 523
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -23,10 +23,9 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: ""
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -52,7 +51,7 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 1
+    - Alpha: 0.5
       Class: rviz/RobotModel
       Collision Enabled: false
       Enabled: true
@@ -230,24 +229,42 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        ultraSound_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true
-    - Class: rviz/Axes
+    - Alpha: 1
+      Class: rviz/Axes
       Enabled: false
       Length: 1
       Name: Axes
       Radius: 0.10000000149011612
       Reference Frame: <Fixed Frame>
+      Show Trail: false
       Value: false
     - Class: rviz/TF
       Enabled: false
       Frame Timeout: 15
       Frames:
         All Enabled: true
+      Marker Alpha: 1
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -272,9 +289,7 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -302,9 +317,7 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -332,9 +345,7 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -362,9 +373,7 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -392,9 +401,7 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -435,33 +442,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.724609613418579
+      Distance: 1.0180186033248901
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.29964715242385864
-        Y: -0.07829883694648743
-        Z: -0.04122250899672508
+        X: 0.10819146782159805
+        Y: 0.02224866859614849
+        Z: -0.08825916796922684
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3647973835468292
+      Pitch: 0.3647974133491516
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 2.6735990047454834
+      Yaw: 4.123597621917725
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 752
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c7000003a7fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000570000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c700000296fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000296000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000042800fffffffb0000000800540069006d006501000000000000045000000000000000000000039d0000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -470,6 +477,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1853
-  X: 1987
-  Y: 27
+  Width: 1386
+  X: 442
+  Y: 159
diff --git a/robots/go1_description/urdf/go1.urdf b/robots/go1_description/urdf/go1.urdf
index 4056823..051002a 100644
--- a/robots/go1_description/urdf/go1.urdf
+++ b/robots/go1_description/urdf/go1.urdf
@@ -3,7 +3,7 @@
 <!-- |    This document was autogenerated by xacro from robot.xacro                    | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
-<robot name="go1" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="go1">
   <material name="black">
     <color rgba="0.0 0.0 0.0 1.0"/>
   </material>
@@ -17,13 +17,13 @@
     <color rgba="0.2 0.2 0.2 1.0"/>
   </material>
   <material name="silver">
-    <color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
+    <color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
   </material>
   <material name="orange">
-    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+    <color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
   </material>
   <material name="brown">
-    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+    <color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
   </material>
   <material name="red">
     <color rgba="0.8 0.0 0.0 1.0"/>
@@ -393,6 +393,8 @@
   </gazebo>
   <!-- <xacro:include filename="$(find go1_gazebo)/launch/stairs.urdf.xacro"/> -->
   <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
+  <!-- Rotor related joint and link is only for demonstrate location. -->
+  <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
   <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
   <!-- <xacro:if value="$(arg DEBUG)">
         <link name="world"/>
@@ -432,7 +434,7 @@
     <inertial>
       <origin rpy="0 0 0" xyz="0.0223 0.002 -0.0005"/>
       <mass value="5.204"/>
-      <inertia ixx="0.0168352186" ixy="0.0004636141" ixz="0.0002367952" iyy="0.0656071082" iyz="3.6671e-05" izz="0.0742720659"/>
+      <inertia ixx="0.0168128557" ixy="-0.0002296769" ixz="-0.0002945293" iyy="0.063009565" iyz="-4.18731e-05" izz="0.0716547275"/>
     </inertial>
   </link>
   <joint name="imu_joint" type="fixed">
@@ -466,41 +468,52 @@
     <child link="FR_hip"/>
     <axis xyz="1 0 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-1.0471975512" upper="1.0471975512" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
+  </joint>
+  <joint name="FR_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.11215 -0.04675 0"/>
+    <parent link="trunk"/>
+    <child link="FR_hip_rotor"/>
   </joint>
   <link name="FR_hip">
     <visual>
-      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
       <geometry>
         <mesh filename="package://go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
       <geometry>
         <cylinder length="0.04" radius="0.046"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.00541 0.00074 6e-06"/>
+      <origin rpy="0 0 0" xyz="-0.005657 0.008752 -0.000102"/>
       <mass value="0.591"/>
-      <inertia ixx="0.000374268192" ixy="-3.6844422e-05" ixz="-9.86754e-07" iyy="0.000635923669" iyz="1.172894e-06" izz="0.000457647394"/>
+      <inertia ixx="0.000334008405" ixy="1.0826066e-05" ixz="1.290732e-06" iyy="0.000619101213" iyz="-1.643194e-06" izz="0.00040057614"/>
     </inertial>
   </link>
-  <joint name="FR_hip_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.08 0"/>
-    <parent link="FR_hip"/>
-    <child link="FR_thigh_shoulder"/>
-  </joint>
-  <!-- this link is only for collision -->
-  <link name="FR_thigh_shoulder">
-    <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+  <link name="FR_hip_rotor">
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
       <geometry>
-        <cylinder length="0.032" radius="0.041"/>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
       </geometry>
     </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
+    </inertial>
   </link>
   <joint name="FR_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 -0.08 0"/>
@@ -508,7 +521,12 @@
     <child link="FR_thigh"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-0.663225115758" upper="2.96705972839" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
+  </joint>
+  <joint name="FR_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.00015 0"/>
+    <parent link="FR_hip"/>
+    <child link="FR_thigh_rotor"/>
   </joint>
   <link name="FR_thigh">
     <visual>
@@ -519,15 +537,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.0245 0.034"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
+      <origin rpy="0 0 0" xyz="-0.003342 0.018054 -0.033451"/>
       <mass value="0.92"/>
-      <inertia ixx="0.005851561134" ixy="-1.783284e-06" ixz="0.000328291374" iyy="0.005596155105" iyz="-2.1430713e-05" izz="0.00107157026"/>
+      <inertia ixx="0.004431760472" ixy="-5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="-0.000572001265" izz="0.000740309489"/>
+    </inertial>
+  </link>
+  <link name="FR_thigh_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="FR_calf_joint" type="revolute">
@@ -536,7 +574,12 @@
     <child link="FR_calf"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-2.72271363311" upper="-0.837758040957" velocity="30.1"/>
+    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
+  </joint>
+  <joint name="FR_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.03235 0"/>
+    <parent link="FR_thigh"/>
+    <child link="FR_calf_rotor"/>
   </joint>
   <link name="FR_calf">
     <visual>
@@ -547,15 +590,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.016 0.016"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
-      <mass value="0.131"/>
-      <inertia ixx="0.002939186297" ixy="1.440899e-06" ixz="-0.00010535955" iyy="0.00295576935" iyz="-2.4397752e-05" izz="3.0273372e-05"/>
+      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
+      <mass value="0.135862"/>
+      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
+    </inertial>
+  </link>
+  <link name="FR_calf_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="FR_foot_fixed" type="fixed">
@@ -579,7 +642,7 @@
     </collision>
     <inertial>
       <mass value="0.06"/>
-      <inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
+      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
     </inertial>
   </link>
   <transmission name="FR_hip_tran">
@@ -618,7 +681,12 @@
     <child link="FL_hip"/>
     <axis xyz="1 0 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-1.0471975512" upper="1.0471975512" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
+  </joint>
+  <joint name="FL_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.11215 0.04675 0"/>
+    <parent link="trunk"/>
+    <child link="FL_hip_rotor"/>
   </joint>
   <link name="FL_hip">
     <visual>
@@ -629,30 +697,36 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
       <geometry>
         <cylinder length="0.04" radius="0.046"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.00541 -0.00074 6e-06"/>
+      <origin rpy="0 0 0" xyz="-0.005657 -0.008752 -0.000102"/>
       <mass value="0.591"/>
-      <inertia ixx="0.000374268192" ixy="3.6844422e-05" ixz="-9.86754e-07" iyy="0.000635923669" iyz="-1.172894e-06" izz="0.000457647394"/>
+      <inertia ixx="0.000334008405" ixy="-1.0826066e-05" ixz="1.290732e-06" iyy="0.000619101213" iyz="1.643194e-06" izz="0.00040057614"/>
     </inertial>
   </link>
-  <joint name="FL_hip_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.08 0"/>
-    <parent link="FL_hip"/>
-    <child link="FL_thigh_shoulder"/>
-  </joint>
-  <!-- this link is only for collision -->
-  <link name="FL_thigh_shoulder">
-    <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+  <link name="FL_hip_rotor">
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
       <geometry>
-        <cylinder length="0.032" radius="0.041"/>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
       </geometry>
     </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
+    </inertial>
   </link>
   <joint name="FL_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0.08 0"/>
@@ -660,7 +734,12 @@
     <child link="FL_thigh"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-0.663225115758" upper="2.96705972839" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
+  </joint>
+  <joint name="FL_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 -0.00015 0"/>
+    <parent link="FL_hip"/>
+    <child link="FL_thigh_rotor"/>
   </joint>
   <link name="FL_thigh">
     <visual>
@@ -671,15 +750,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.0245 0.034"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
+      <origin rpy="0 0 0" xyz="-0.003342 -0.018054 -0.033451"/>
       <mass value="0.92"/>
-      <inertia ixx="0.005851561134" ixy="1.783284e-06" ixz="0.000328291374" iyy="0.005596155105" iyz="2.1430713e-05" izz="0.00107157026"/>
+      <inertia ixx="0.004431760472" ixy="5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="0.000572001265" izz="0.000740309489"/>
+    </inertial>
+  </link>
+  <link name="FL_thigh_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="FL_calf_joint" type="revolute">
@@ -688,7 +787,12 @@
     <child link="FL_calf"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-2.72271363311" upper="-0.837758040957" velocity="30.1"/>
+    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
+  </joint>
+  <joint name="FL_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 -0.03235 0"/>
+    <parent link="FL_thigh"/>
+    <child link="FL_calf_rotor"/>
   </joint>
   <link name="FL_calf">
     <visual>
@@ -699,15 +803,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.016 0.016"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
-      <mass value="0.131"/>
-      <inertia ixx="0.002939186297" ixy="1.440899e-06" ixz="-0.00010535955" iyy="0.00295576935" iyz="-2.4397752e-05" izz="3.0273372e-05"/>
+      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
+      <mass value="0.135862"/>
+      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
+    </inertial>
+  </link>
+  <link name="FL_calf_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="FL_foot_fixed" type="fixed">
@@ -731,7 +855,7 @@
     </collision>
     <inertial>
       <mass value="0.06"/>
-      <inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
+      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
     </inertial>
   </link>
   <transmission name="FL_hip_tran">
@@ -770,41 +894,52 @@
     <child link="RR_hip"/>
     <axis xyz="1 0 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-1.0471975512" upper="1.0471975512" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
+  </joint>
+  <joint name="RR_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.11215 -0.04675 0"/>
+    <parent link="trunk"/>
+    <child link="RR_hip_rotor"/>
   </joint>
   <link name="RR_hip">
     <visual>
-      <origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
+      <origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
       <geometry>
         <mesh filename="package://go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
       <geometry>
         <cylinder length="0.04" radius="0.046"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.00541 0.00074 6e-06"/>
+      <origin rpy="0 0 0" xyz="0.005657 0.008752 -0.000102"/>
       <mass value="0.591"/>
-      <inertia ixx="0.000374268192" ixy="3.6844422e-05" ixz="9.86754e-07" iyy="0.000635923669" iyz="1.172894e-06" izz="0.000457647394"/>
+      <inertia ixx="0.000334008405" ixy="-1.0826066e-05" ixz="-1.290732e-06" iyy="0.000619101213" iyz="-1.643194e-06" izz="0.00040057614"/>
     </inertial>
   </link>
-  <joint name="RR_hip_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.08 0"/>
-    <parent link="RR_hip"/>
-    <child link="RR_thigh_shoulder"/>
-  </joint>
-  <!-- this link is only for collision -->
-  <link name="RR_thigh_shoulder">
-    <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+  <link name="RR_hip_rotor">
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
       <geometry>
-        <cylinder length="0.032" radius="0.041"/>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
       </geometry>
     </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
+    </inertial>
   </link>
   <joint name="RR_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 -0.08 0"/>
@@ -812,7 +947,12 @@
     <child link="RR_thigh"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-0.663225115758" upper="2.96705972839" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
+  </joint>
+  <joint name="RR_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.00015 0"/>
+    <parent link="RR_hip"/>
+    <child link="RR_thigh_rotor"/>
   </joint>
   <link name="RR_thigh">
     <visual>
@@ -823,15 +963,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.0245 0.034"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
+      <origin rpy="0 0 0" xyz="-0.003342 0.018054 -0.033451"/>
       <mass value="0.92"/>
-      <inertia ixx="0.005851561134" ixy="-1.783284e-06" ixz="0.000328291374" iyy="0.005596155105" iyz="-2.1430713e-05" izz="0.00107157026"/>
+      <inertia ixx="0.004431760472" ixy="-5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="-0.000572001265" izz="0.000740309489"/>
+    </inertial>
+  </link>
+  <link name="RR_thigh_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="RR_calf_joint" type="revolute">
@@ -840,7 +1000,12 @@
     <child link="RR_calf"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-2.72271363311" upper="-0.837758040957" velocity="30.1"/>
+    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
+  </joint>
+  <joint name="RR_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.03235 0"/>
+    <parent link="RR_thigh"/>
+    <child link="RR_calf_rotor"/>
   </joint>
   <link name="RR_calf">
     <visual>
@@ -851,15 +1016,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.016 0.016"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
-      <mass value="0.131"/>
-      <inertia ixx="0.002939186297" ixy="1.440899e-06" ixz="-0.00010535955" iyy="0.00295576935" iyz="-2.4397752e-05" izz="3.0273372e-05"/>
+      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
+      <mass value="0.135862"/>
+      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
+    </inertial>
+  </link>
+  <link name="RR_calf_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="RR_foot_fixed" type="fixed">
@@ -883,7 +1068,7 @@
     </collision>
     <inertial>
       <mass value="0.06"/>
-      <inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
+      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
     </inertial>
   </link>
   <transmission name="RR_hip_tran">
@@ -922,41 +1107,52 @@
     <child link="RL_hip"/>
     <axis xyz="1 0 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-1.0471975512" upper="1.0471975512" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.863" upper="0.863" velocity="30.1"/>
+  </joint>
+  <joint name="RL_hip_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.11215 0.04675 0"/>
+    <parent link="trunk"/>
+    <child link="RL_hip_rotor"/>
   </joint>
   <link name="RL_hip">
     <visual>
-      <origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
+      <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
       <geometry>
         <mesh filename="package://go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
       <geometry>
         <cylinder length="0.04" radius="0.046"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.00541 -0.00074 6e-06"/>
+      <origin rpy="0 0 0" xyz="0.005657 -0.008752 -0.000102"/>
       <mass value="0.591"/>
-      <inertia ixx="0.000374268192" ixy="-3.6844422e-05" ixz="9.86754e-07" iyy="0.000635923669" iyz="-1.172894e-06" izz="0.000457647394"/>
+      <inertia ixx="0.000334008405" ixy="1.0826066e-05" ixz="-1.290732e-06" iyy="0.000619101213" iyz="1.643194e-06" izz="0.00040057614"/>
     </inertial>
   </link>
-  <joint name="RL_hip_fixed" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.08 0"/>
-    <parent link="RL_hip"/>
-    <child link="RL_thigh_shoulder"/>
-  </joint>
-  <!-- this link is only for collision -->
-  <link name="RL_thigh_shoulder">
-    <collision>
-      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
+  <link name="RL_hip_rotor">
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
       <geometry>
-        <cylinder length="0.032" radius="0.041"/>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
       </geometry>
     </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="0.000111842" ixy="0.0" ixz="0.0" iyy="5.9647e-05" iyz="0.0" izz="5.9647e-05"/>
+    </inertial>
   </link>
   <joint name="RL_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0.08 0"/>
@@ -964,7 +1160,12 @@
     <child link="RL_thigh"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-0.663225115758" upper="2.96705972839" velocity="30.1"/>
+    <limit effort="23.7" lower="-0.686" upper="4.501" velocity="30.1"/>
+  </joint>
+  <joint name="RL_thigh_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 -0.00015 0"/>
+    <parent link="RL_hip"/>
+    <child link="RL_thigh_rotor"/>
   </joint>
   <link name="RL_thigh">
     <visual>
@@ -975,15 +1176,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.0245 0.034"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
+      <origin rpy="0 0 0" xyz="-0.003342 -0.018054 -0.033451"/>
       <mass value="0.92"/>
-      <inertia ixx="0.005851561134" ixy="1.783284e-06" ixz="0.000328291374" iyy="0.005596155105" iyz="2.1430713e-05" izz="0.00107157026"/>
+      <inertia ixx="0.004431760472" ixy="5.7496807e-05" ixz="-0.000218457134" iyy="0.004485671726" iyz="0.000572001265" izz="0.000740309489"/>
+    </inertial>
+  </link>
+  <link name="RL_thigh_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="RL_calf_joint" type="revolute">
@@ -992,7 +1213,12 @@
     <child link="RL_calf"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="0.01" friction="0.2"/>
-    <limit effort="23.7" lower="-2.72271363311" upper="-0.837758040957" velocity="30.1"/>
+    <limit effort="35.55" lower="-2.818" upper="-0.888" velocity="20.06"/>
+  </joint>
+  <joint name="RL_calf_rotor_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 -0.03235 0"/>
+    <parent link="RL_thigh"/>
+    <child link="RL_calf_rotor"/>
   </joint>
   <link name="RL_calf">
     <visual>
@@ -1003,15 +1229,35 @@
       <!-- <material name="orange"/> -->
     </visual>
     <collision>
-      <origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
+      <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
       <geometry>
         <box size="0.213 0.016 0.016"/>
       </geometry>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
-      <mass value="0.131"/>
-      <inertia ixx="0.002939186297" ixy="1.440899e-06" ixz="-0.00010535955" iyy="0.00295576935" iyz="-2.4397752e-05" izz="3.0273372e-05"/>
+      <origin rpy="0 0 0" xyz="0.006197 0.001408 -0.116695"/>
+      <mass value="0.135862"/>
+      <inertia ixx="0.001088793059" ixy="-2.55679e-07" ixz="7.117814e-06" iyy="0.001100428748" iyz="2.077264e-06" izz="2.4787446e-05"/>
+    </inertial>
+  </link>
+  <link name="RL_calf_rotor">
+    <visual>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.089"/>
+      <inertia ixx="5.9647e-05" ixy="0.0" ixz="0.0" iyy="0.000111842" iyz="0.0" izz="5.9647e-05"/>
     </inertial>
   </link>
   <joint name="RL_foot_fixed" type="fixed">
@@ -1035,7 +1281,7 @@
     </collision>
     <inertial>
       <mass value="0.06"/>
-      <inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
+      <inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
     </inertial>
   </link>
   <transmission name="RL_hip_tran">
@@ -1069,7 +1315,7 @@
     </actuator>
   </transmission>
   <joint name="camera_joint_face" type="fixed">
-    <origin rpy="3.14159265359 0 0" xyz="0.2785 0.0125 0.0167"/>
+    <origin rpy="3.141592653589793 0 0" xyz="0.2785 0.0125 0.0167"/>
     <parent link="trunk"/>
     <child link="camera_face"/>
   </joint>
@@ -1093,7 +1339,7 @@
     </inertial>
   </link>
   <joint name="camera_optical_joint_face" type="fixed">
-    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
     <parent link="camera_face"/>
     <child link="camera_optical_face"/>
   </joint>
@@ -1143,7 +1389,7 @@
     </sensor>
   </gazebo>
   <joint name="camera_joint_chin" type="fixed">
-    <origin rpy="3.14159265359 1.57079632679 0" xyz="0.2522 0.0125 -0.0436"/>
+    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="0.2522 0.0125 -0.0436"/>
     <parent link="trunk"/>
     <child link="camera_chin"/>
   </joint>
@@ -1167,7 +1413,7 @@
     </inertial>
   </link>
   <joint name="camera_optical_joint_chin" type="fixed">
-    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
     <parent link="camera_chin"/>
     <child link="camera_optical_chin"/>
   </joint>
@@ -1217,7 +1463,7 @@
     </sensor>
   </gazebo>
   <joint name="camera_joint_left" type="fixed">
-    <origin rpy="3.14159265359 0.2618 1.57079632679" xyz="-0.066 0.082 -0.0176"/>
+    <origin rpy="3.141592653589793 0.2618 1.5707963267948966" xyz="-0.066 0.082 -0.0176"/>
     <parent link="trunk"/>
     <child link="camera_left"/>
   </joint>
@@ -1241,7 +1487,7 @@
     </inertial>
   </link>
   <joint name="camera_optical_joint_left" type="fixed">
-    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
     <parent link="camera_left"/>
     <child link="camera_optical_left"/>
   </joint>
@@ -1291,7 +1537,7 @@
     </sensor>
   </gazebo>
   <joint name="camera_joint_right" type="fixed">
-    <origin rpy="3.14159265359 0.2618 -1.57079632679" xyz="-0.041 -0.082 -0.0176"/>
+    <origin rpy="3.141592653589793 0.2618 -1.5707963267948966" xyz="-0.041 -0.082 -0.0176"/>
     <parent link="trunk"/>
     <child link="camera_right"/>
   </joint>
@@ -1315,7 +1561,7 @@
     </inertial>
   </link>
   <joint name="camera_optical_joint_right" type="fixed">
-    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
     <parent link="camera_right"/>
     <child link="camera_optical_right"/>
   </joint>
@@ -1365,7 +1611,7 @@
     </sensor>
   </gazebo>
   <joint name="camera_joint_rearDown" type="fixed">
-    <origin rpy="3.14159265359 1.57079632679 0" xyz="-0.0825 0.0125 -0.04365"/>
+    <origin rpy="3.141592653589793 1.5707963267948966 0" xyz="-0.0825 0.0125 -0.04365"/>
     <parent link="trunk"/>
     <child link="camera_rearDown"/>
   </joint>
@@ -1389,7 +1635,7 @@
     </inertial>
   </link>
   <joint name="camera_optical_joint_rearDown" type="fixed">
-    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
     <parent link="camera_rearDown"/>
     <child link="camera_optical_rearDown"/>
   </joint>
@@ -1453,7 +1699,7 @@
   <link name="camera_laserscan_link_right">
     </link>
   <joint name="ultraSound_joint_left" type="fixed">
-    <origin rpy="0 0.2618 1.57079632679" xyz="-0.0535  0.0826 0.00868"/>
+    <origin rpy="0 0.2618 1.5707963267948966" xyz="-0.0535  0.0826 0.00868"/>
     <parent link="trunk"/>
     <child link="ultraSound_left"/>
   </joint>
@@ -1478,7 +1724,7 @@
     </inertial>
   </link>
   <joint name="ultraSound_joint_right" type="fixed">
-    <origin rpy="0 0.2618 -1.57079632679" xyz="-0.0535 -0.0826 0.00868"/>
+    <origin rpy="0 0.2618 -1.5707963267948966" xyz="-0.0535 -0.0826 0.00868"/>
     <parent link="trunk"/>
     <child link="ultraSound_right"/>
   </joint>
diff --git a/robots/go1_description/xacro/const.xacro b/robots/go1_description/xacro/const.xacro
index 4363175..efa665f 100644
--- a/robots/go1_description/xacro/const.xacro
+++ b/robots/go1_description/xacro/const.xacro
@@ -33,21 +33,43 @@
     <!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
     <xacro:property name="hip_offset" value="0.08"/>
 
+    <!-- offset of link and rotor locations (left front) -->
+    <xacro:property name="hip_offset_x" value="0.1881"/>
+    <xacro:property name="hip_offset_y" value="0.04675"/>
+    <xacro:property name="hip_offset_z" value="0.0"/>
+    <xacro:property name="hip_rotor_offset_x" value="0.11215"/>
+    <xacro:property name="hip_rotor_offset_y" value="0.04675"/>
+    <xacro:property name="hip_rotor_offset_z" value="0.0"/>
+
+    <xacro:property name="thigh_offset_x" value="0"/>
+    <xacro:property name="thigh_offset_y" value="0.08"/>
+    <xacro:property name="thigh_offset_z" value="0.0"/>
+    <xacro:property name="thigh_rotor_offset_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_offset_y" value="-0.00015"/>
+    <xacro:property name="thigh_rotor_offset_z" value="0.0"/>
+
+    <xacro:property name="calf_offset_x" value="0.0"/>
+    <xacro:property name="calf_offset_y" value="0.0"/>
+    <xacro:property name="calf_offset_z" value="-0.213"/>
+    <xacro:property name="calf_rotor_offset_x" value="0.0"/>
+    <xacro:property name="calf_rotor_offset_y" value="-0.03235"/>
+    <xacro:property name="calf_rotor_offset_z" value="0.0"/>
+
     <!-- joint limits -->
     <xacro:property name="damping" value="0.01"/>
     <xacro:property name="friction" value="0.2"/>
-    <xacro:property name="hip_max" value="60"/>
-    <xacro:property name="hip_min" value="-60"/>
+    <xacro:property name="hip_position_max" value="0.863"/>
+    <xacro:property name="hip_position_min" value="-0.863"/>
     <xacro:property name="hip_velocity_max" value="30.1"/>
     <xacro:property name="hip_torque_max" value="23.7"/>
-    <xacro:property name="thigh_max" value="170"/>
-    <xacro:property name="thigh_min" value="-38"/>
+    <xacro:property name="thigh_position_max" value="4.501"/>
+    <xacro:property name="thigh_position_min" value="-0.686"/>
     <xacro:property name="thigh_velocity_max" value="30.1"/>
     <xacro:property name="thigh_torque_max" value="23.7"/>
-    <xacro:property name="calf_max" value="-48"/>
-    <xacro:property name="calf_min" value="-156"/>
-    <xacro:property name="calf_velocity_max" value="30.1"/>
-    <xacro:property name="calf_torque_max" value="23.7"/>
+    <xacro:property name="calf_position_max" value="-0.888"/>
+    <xacro:property name="calf_position_min" value="-2.818"/>
+    <xacro:property name="calf_velocity_max" value="20.06"/>
+    <xacro:property name="calf_torque_max" value="35.55"/>
 
     <!-- dynamics inertial value        total 12.84kg -->
     <!-- trunk -->
@@ -55,48 +77,81 @@
     <xacro:property name="trunk_com_x" value="0.0223"/>
     <xacro:property name="trunk_com_y" value="0.002"/>
     <xacro:property name="trunk_com_z" value="-0.0005"/>
-    <xacro:property name="trunk_ixx" value="0.0168352186"/>
-    <xacro:property name="trunk_ixy" value="0.0004636141"/>
-    <xacro:property name="trunk_ixz" value="0.0002367952"/>
-    <xacro:property name="trunk_iyy" value="0.0656071082"/>
-    <xacro:property name="trunk_iyz" value="0.000036671"/>
-    <xacro:property name="trunk_izz" value="0.0742720659"/>
+    <xacro:property name="trunk_ixx" value="0.0168128557"/>
+    <xacro:property name="trunk_ixy" value="-0.0002296769"/>
+    <xacro:property name="trunk_ixz" value="-0.0002945293"/>
+    <xacro:property name="trunk_iyy" value="0.063009565"/>
+    <xacro:property name="trunk_iyz" value="-0.0000418731"/>
+    <xacro:property name="trunk_izz" value="0.0716547275"/>
 
     <!-- hip (left front) -->
     <xacro:property name="hip_mass" value="0.591"/>
-    <xacro:property name="hip_com_x" value="-0.00541"/>
-    <xacro:property name="hip_com_y" value="-0.00074"/>
-    <xacro:property name="hip_com_z" value="0.000006"/>
-    <xacro:property name="hip_ixx" value="0.000374268192"/>
-    <xacro:property name="hip_ixy" value="0.000036844422"/>
-    <xacro:property name="hip_ixz" value="-0.000000986754"/>
-    <xacro:property name="hip_iyy" value="0.000635923669"/>
-    <xacro:property name="hip_iyz" value="-0.000001172894"/>
-    <xacro:property name="hip_izz" value="0.000457647394"/>
+    <xacro:property name="hip_com_x" value="-0.005657"/>
+    <xacro:property name="hip_com_y" value="-0.008752"/>
+    <xacro:property name="hip_com_z" value="-0.000102"/>
+    <xacro:property name="hip_ixx" value="0.000334008405"/>
+    <xacro:property name="hip_ixy" value="-0.000010826066"/>
+    <xacro:property name="hip_ixz" value="0.000001290732"/>
+    <xacro:property name="hip_iyy" value="0.000619101213"/>
+    <xacro:property name="hip_iyz" value="0.000001643194"/>
+    <xacro:property name="hip_izz" value="0.00040057614"/>
+
+    <xacro:property name="hip_rotor_mass" value="0.089"/>
+    <xacro:property name="hip_rotor_com_x" value="0.0"/>
+    <xacro:property name="hip_rotor_com_y" value="0.0"/>
+    <xacro:property name="hip_rotor_com_z" value="0.0"/>
+    <xacro:property name="hip_rotor_ixx" value="0.000111842"/>
+    <xacro:property name="hip_rotor_ixy" value="0.0"/>
+    <xacro:property name="hip_rotor_ixz" value="0.0"/>
+    <xacro:property name="hip_rotor_iyy" value="0.000059647"/>
+    <xacro:property name="hip_rotor_iyz" value="0.0"/>
+    <xacro:property name="hip_rotor_izz" value="0.000059647"/>
 
     <!-- thigh -->
     <xacro:property name="thigh_mass" value="0.92"/>
-    <xacro:property name="thigh_com_x" value="-0.003468"/>
-    <xacro:property name="thigh_com_y" value="-0.018947"/>
-    <xacro:property name="thigh_com_z" value="-0.032736"/>
-    <xacro:property name="thigh_ixx" value="0.005851561134"/>
-    <xacro:property name="thigh_ixy" value="0.000001783284"/>
-    <xacro:property name="thigh_ixz" value="0.000328291374"/>
-    <xacro:property name="thigh_iyy" value="0.005596155105"/>
-    <xacro:property name="thigh_iyz" value="0.000021430713"/>
-    <xacro:property name="thigh_izz" value="0.00107157026"/>
+    <xacro:property name="thigh_com_x" value="-0.003342"/>
+    <xacro:property name="thigh_com_y" value="-0.018054"/>
+    <xacro:property name="thigh_com_z" value="-0.033451"/>
+    <xacro:property name="thigh_ixx" value="0.004431760472"/>
+    <xacro:property name="thigh_ixy" value="0.000057496807"/>
+    <xacro:property name="thigh_ixz" value="-0.000218457134"/>
+    <xacro:property name="thigh_iyy" value="0.004485671726"/>
+    <xacro:property name="thigh_iyz" value="0.000572001265"/>
+    <xacro:property name="thigh_izz" value="0.000740309489"/>
+
+    <xacro:property name="thigh_rotor_mass" value="0.089"/>
+    <xacro:property name="thigh_rotor_com_x" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_y" value="0.0"/>
+    <xacro:property name="thigh_rotor_com_z" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixx" value="0.000059647"/>
+    <xacro:property name="thigh_rotor_ixy" value="0.0"/>
+    <xacro:property name="thigh_rotor_ixz" value="0.0"/>
+    <xacro:property name="thigh_rotor_iyy" value="0.000111842"/>
+    <xacro:property name="thigh_rotor_iyz" value="0.0"/>
+    <xacro:property name="thigh_rotor_izz" value="0.000059647"/>
 
     <!-- calf -->
-    <xacro:property name="calf_mass" value="0.131"/>
-    <xacro:property name="calf_com_x" value="0.006286"/>
-    <xacro:property name="calf_com_y" value="0.001307"/>
-    <xacro:property name="calf_com_z" value="-0.122269"/>
-    <xacro:property name="calf_ixx" value="0.002939186297"/>
-    <xacro:property name="calf_ixy" value="0.000001440899"/>
-    <xacro:property name="calf_ixz" value="-0.000105359550"/>
-    <xacro:property name="calf_iyy" value="0.00295576935"/>
-    <xacro:property name="calf_iyz" value="-0.000024397752"/>
-    <xacro:property name="calf_izz" value="0.000030273372"/>
+    <xacro:property name="calf_mass" value="0.135862"/>
+    <xacro:property name="calf_com_x" value="0.006197"/>
+    <xacro:property name="calf_com_y" value="0.001408"/>
+    <xacro:property name="calf_com_z" value="-0.116695"/>
+    <xacro:property name="calf_ixx" value="0.001088793059"/>
+    <xacro:property name="calf_ixy" value="-0.000000255679"/>
+    <xacro:property name="calf_ixz" value="0.000007117814"/>
+    <xacro:property name="calf_iyy" value="0.001100428748"/>
+    <xacro:property name="calf_iyz" value="0.000002077264"/>
+    <xacro:property name="calf_izz" value="0.000024787446"/>
+
+    <xacro:property name="calf_rotor_mass" value="0.089"/>
+    <xacro:property name="calf_rotor_com_x" value="0.0"/>
+    <xacro:property name="calf_rotor_com_y" value="0.0"/>
+    <xacro:property name="calf_rotor_com_z" value="0.0"/>
+    <xacro:property name="calf_rotor_ixx" value="0.000059647"/>
+    <xacro:property name="calf_rotor_ixy" value="0.0"/>
+    <xacro:property name="calf_rotor_ixz" value="0.0"/>
+    <xacro:property name="calf_rotor_iyy" value="0.000111842"/>
+    <xacro:property name="calf_rotor_iyz" value="0.0"/>
+    <xacro:property name="calf_rotor_izz" value="0.000059647"/>
 
     <!-- foot -->
     <xacro:property name="foot_mass" value="0.06"/>
diff --git a/robots/go1_description/xacro/leg.xacro b/robots/go1_description/xacro/leg.xacro
index f4fe0b7..f49b9d6 100644
--- a/robots/go1_description/xacro/leg.xacro
+++ b/robots/go1_description/xacro/leg.xacro
@@ -4,22 +4,50 @@
 
     <xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
 
-    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
+    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
 
         <joint name="${name}_hip_joint" type="revolute">
-            <xacro:insert_block name="origin"/>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+            </xacro:if>
             <parent link="trunk"/>
             <child link="${name}_hip"/>
             <axis xyz="1 0 0"/>
             <dynamics damping="${damping}" friction="${friction}"/>
             <xacro:if value="${(mirror_dae == True)}">
-                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_min*PI/180.0}" upper="${hip_max*PI/180.0}"/>
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
             </xacro:if>
             <xacro:if value="${(mirror_dae == False)}">
-                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_max*PI/180.0}" upper="${-hip_min*PI/180.0}"/>
+                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
             </xacro:if>            
         </joint>
 
+        <joint name="${name}_hip_rotor_joint" type="fixed">
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+            </xacro:if>
+            <parent link="trunk"/>
+            <child link="${name}_hip_rotor"/>         
+        </joint>
+
         <link name="${name}_hip">
             <visual>
                 <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
@@ -40,7 +68,7 @@
                 <!-- <material name="orange"/> -->
             </visual>
             <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
                 <geometry>
                     <cylinder length="${hip_length}" radius="${hip_radius}"/>
                 </geometry>
@@ -55,20 +83,28 @@
             </inertial>
         </link>
 
-        <joint name="${name}_hip_fixed" type="fixed">
-            <origin rpy="0 0 0" xyz="0 ${(hip_offset)*mirror} 0"/>
-            <parent link="${name}_hip"/>
-            <child link="${name}_thigh_shoulder"/>
-        </joint>
-
-        <!-- this link is only for collision -->
-        <link name="${name}_thigh_shoulder">
-            <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <link name="${name}_hip_rotor">
+            <visual>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
                 <geometry>
-                    <cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <collision>
+                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
                 </geometry>
             </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+                <mass value="${hip_rotor_mass}"/>
+                <inertia
+                    ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+                    iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+                    izz="${hip_rotor_izz}"/>              
+            </inertial>
         </link>
 
         <joint name="${name}_thigh_joint" type="revolute">
@@ -77,7 +113,13 @@
             <child link="${name}_thigh"/>
             <axis xyz="0 1 0"/>
             <dynamics damping="${damping}" friction="${friction}"/>
-            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_min*PI/180.0}" upper="${thigh_max*PI/180.0}"/>
+            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
+        </joint>
+
+        <joint name="${name}_thigh_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_hip"/>
+            <child link="${name}_thigh_rotor"/>         
         </joint>
 
         <link name="${name}_thigh">
@@ -109,13 +151,43 @@
             </inertial>
         </link>
 
+        <link name="${name}_thigh_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+                <mass value="${thigh_rotor_mass}"/>
+                <inertia
+                    ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+                    iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+                    izz="${thigh_rotor_izz}"/>       
+            </inertial>
+        </link>
+
         <joint name="${name}_calf_joint" type="revolute">
             <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
             <parent link="${name}_thigh"/>
             <child link="${name}_calf"/>
             <axis xyz="0 1 0"/>
             <dynamics damping="${damping}" friction="${friction}"/>
-            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_min*PI/180.0}" upper="${calf_max*PI/180.0}"/>
+            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
+        </joint>
+
+        <joint name="${name}_calf_rotor_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+            <parent link="${name}_thigh"/>
+            <child link="${name}_calf_rotor"/>         
         </joint>
 
         <link name="${name}_calf">
@@ -142,6 +214,30 @@
             </inertial>
         </link>
 
+        <link name="${name}_calf_rotor">
+            <visual>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+            <collision>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <cylinder length="0.02" radius="0.035"/>
+                </geometry>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+                <mass value="${calf_rotor_mass}"/>
+                <inertia
+                    ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+                    iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+                    izz="${calf_rotor_izz}"/>     
+            </inertial>
+        </link>
+
         <joint name="${name}_foot_fixed" type="fixed">
             <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
             <parent link="${name}_calf"/>
diff --git a/robots/go1_description/xacro/robot.xacro b/robots/go1_description/xacro/robot.xacro
index 4c95987..6425f9c 100644
--- a/robots/go1_description/xacro/robot.xacro
+++ b/robots/go1_description/xacro/robot.xacro
@@ -15,18 +15,18 @@
 
     <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
 
-    <!-- Rollover Protection mode will add an additional stick on the top, use "true" or "false" to switch it. -->
-    <xacro:property name="rolloverProtection" value="false"/>
+    <!-- Rotor related joint and link is only for demonstrate location. -->
+    <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
 
     <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
-    <xacro:if value="$(arg DEBUG)">
+    <!-- <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> 
+    </xacro:if>  -->
 
     <link name="base">
         <visual>
@@ -67,37 +67,6 @@
         </inertial>
     </link>
 
-    <xacro:if value="${(rolloverProtection == 'true')}">
-        <joint name="stick_joint" type="fixed">
-            <parent link="trunk"/>
-            <child link="stick_link"/>
-            <origin rpy="0 0 0" xyz="${0.18} 0 ${stick_length/2.0+0.08}"/>
-        </joint>
-
-        <link name="stick_link">
-            <visual>
-                <geometry>
-                    <cylinder length="${stick_length}" radius="${stick_radius}"/>
-                </geometry>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <!-- <material name="white"/> -->
-            </visual>
-            <collision>
-                <geometry>
-                    <cylinder length="${stick_length}" radius="${stick_radius}"/>
-                </geometry>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-            </collision>
-            <inertial>
-                <mass value="${stick_mass}"/>
-                <inertia
-                    ixx="${stick_mass / 2.0 * (stick_radius*stick_radius)}" ixy="0.0" ixz="0.0"
-                    iyy="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}" iyz="0.0"
-                    izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/>                   
-            </inertial>
-        </link>
-    </xacro:if> 
-
     <joint name="imu_joint" type="fixed">
         <parent link="trunk"/>
         <child link="imu_link"/>
@@ -125,21 +94,10 @@
         </collision>
     </link>
 
-    <xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
-        <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
-    </xacro:leg>
-
-    <xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
-        <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
-    </xacro:leg>
-
-    <xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
-        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
-    </xacro:leg>
-
-    <xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
-        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
-    </xacro:leg>
+    <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
+    <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
+    <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />
 
     <xacro:depthCamera camID="1" name="face">
         <origin rpy="${PI} 0 0" xyz="0.2785 0.0125 0.0167"/>
diff --git a/robots/laikago_description/xacro/robot.xacro b/robots/laikago_description/xacro/robot.xacro
index 339cba6..57e668f 100644
--- a/robots/laikago_description/xacro/robot.xacro
+++ b/robots/laikago_description/xacro/robot.xacro
@@ -102,7 +102,7 @@
         <inertial>
             <mass value="0.001"/>
             <origin rpy="0 0 0" xyz="0 0 0"/>
-            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
         </inertial>
         <visual>
             <origin rpy="0 0 0" xyz="0 0 0"/>