diff --git a/src/robots/b2w_description/urdf/b2w_description.urdf b/src/robots/b2w_description/urdf/b2w_description.urdf
index 4a3de7e..58f7831 100755
--- a/src/robots/b2w_description/urdf/b2w_description.urdf
+++ b/src/robots/b2w_description/urdf/b2w_description.urdf
@@ -2,566 +2,502 @@
 <!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
      Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="b2w_description">
-  <link
-    name="base_link">
+<robot name="b2w_description">
+<material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="silver">
+    <color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <material name="brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <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>/b2w_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>
+
+  <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="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>
+  <link name="base">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="floating_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base"/>
+    <child link="trunk"/>
+  </joint>
+  <link name="trunk">
     <inertial>
-      <origin
-        xyz="0.000458 0.005261 0.000665"
-        rpy="0 0 0" />
-      <mass
-        value="35.606" />
-      <inertia
-        ixx="0.27466"
-        ixy="-0.000622"
-        ixz="0.00315"
-        iyy="1.0618"
-        iyz="-0.00139"
-        izz="1.1825" />
+      <origin xyz="0.000458 0.005261 0.000665" rpy="0 0 0" />
+      <mass value="35.606" />
+      <inertia ixx="0.27466" ixy="-0.000622" ixz="0.00315" iyy="1.0618" iyz="-0.00139" izz="1.1825" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/base_link.dae" />
+        <mesh filename="package://b2w_description/meshes/base_link.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-        <geometry>
-          <box size="0.5 0.28 0.15" />
-        </geometry>
-    </collision>
-  </link>
-  <link
-    name="lidar_link">
-    <inertial>
-      <origin
-        xyz="0.34218 0 0.17851"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-3.2E-45"
-        ixz="7.68E-44"
-        iyy="1.68E-27"
-        iyz="-9.26E-47"
-        izz="1.68E-27" />
-    </inertial>
-    <collision>
-      <origin
-        xyz="0 0 -0.02"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <cylinder length="0.16" radius="0.076" />
-        <!-- <mesh
-          filename="package://b2_description/meshes/lidar_link.STL" /> -->
+        <box size="0.5 0.28 0.15" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="joint_lidar"
-    type="fixed">
-    <origin
-      xyz="0.34218 0 0.17851"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="lidar_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="f_dc_link">
+  <link name="lidar_link">
     <inertial>
-      <origin
-        xyz="3.47E-18 0 5.55E-17"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-9.81E-45"
-        ixz="-2.54E-44"
-        iyy="1.68E-27"
-        iyz="0"
-        izz="1.68E-27" />
-    </inertial>
-  </link>
-  <joint
-    name="joint_f_dc"
-    type="fixed">
-    <origin
-      xyz="0.42161 0.025 0.061851"
-      rpy="-2.3562 0 -1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="f_dc_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="r_dc_link">
-    <inertial>
-      <origin
-        xyz="-7.46E-16 -3.33E-16 -1.11E-15"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-4.31E-44"
-        ixz="2.48E-45"
-        iyy="1.68E-27"
-        iyz="0"
-        izz="1.68E-27" />
-    </inertial>
-  </link>
-  <joint
-    name="joint_r_dc"
-    type="fixed">
-    <origin
-      xyz="-0.42244 -0.025 0.038026"
-      rpy="-2.3562 0 1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="r_dc_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="imu_link">
-    <inertial>
-      <origin
-        xyz="-2.02E-17 1.73E-17 -1.39E-17"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="1.84E-44"
-        ixz="-7.35E-44"
-        iyy="1.68E-27"
-        iyz="-3.67E-44"
-        izz="1.68E-27" />
-    </inertial>
-  </link>
-  <joint
-    name="joint_imu"
-    type="fixed">
-    <origin
-      xyz="0 -0.02341 0.04927"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="imu_link" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  <link
-    name="head_Link">
-    <inertial>
-      <origin
-        xyz="0.33989 -0.000168 0.12029"
-        rpy="0 0 0" />
-      <mass
-        value="3.6885" />
-      <inertia
-        ixx="0.026455"
-        ixy="3.15E-05"
-        ixz="0.00166"
-        iyy="0.029221"
-        iyz="-3.27E-05"
-        izz="0.010918" />
+      <origin xyz="0.34218 0 0.17851" rpy="0 0 0" />
+      <mass value="4.19E-15" />
+      <inertia ixx="1.68E-27" ixy="-3.2E-45" ixz="7.68E-44" iyy="1.68E-27" iyz="-9.26E-47" izz="1.68E-27" />
     </inertial>
     <collision>
-      <origin
-        xyz="0.41 0 0.005"
-        rpy="0 0 0" />
+      <origin xyz="0 0 -0.02" rpy="0 0 0" />
+      <geometry>
+        <cylinder length="0.16" radius="0.076" />
+        <!-- <mesh filename="package://b2_description/meshes/lidar_link.STL" /> -->
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_lidar" type="fixed">
+    <origin xyz="0.34218 0 0.17851" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="lidar_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  <link name="f_dc_link">
+    <inertial>
+      <origin xyz="3.47E-18 0 5.55E-17" rpy="0 0 0" />
+      <mass value="4.19E-15" />
+      <inertia ixx="1.68E-27" ixy="-9.81E-45" ixz="-2.54E-44" iyy="1.68E-27" iyz="0" izz="1.68E-27" />
+    </inertial>
+  </link>
+  <joint name="joint_f_dc" type="fixed">
+    <origin xyz="0.42161 0.025 0.061851" rpy="-2.3562 0 -1.5708" />
+    <parent link="trunk" />
+    <child link="f_dc_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  <link name="r_dc_link">
+    <inertial>
+      <origin xyz="-7.46E-16 -3.33E-16 -1.11E-15" rpy="0 0 0" />
+      <mass value="4.19E-15" />
+      <inertia ixx="1.68E-27" ixy="-4.31E-44" ixz="2.48E-45" iyy="1.68E-27" iyz="0" izz="1.68E-27" />
+    </inertial>
+  </link>
+  <joint name="joint_r_dc" type="fixed">
+    <origin xyz="-0.42244 -0.025 0.038026" rpy="-2.3562 0 1.5708" />
+    <parent link="trunk" />
+    <child link="r_dc_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  <link name="imu_link">
+    <inertial>
+      <origin xyz="-2.02E-17 1.73E-17 -1.39E-17" rpy="0 0 0" />
+      <mass value="4.19E-15" />
+      <inertia ixx="1.68E-27" ixy="1.84E-44" ixz="-7.35E-44" iyy="1.68E-27" iyz="-3.67E-44" izz="1.68E-27" />
+    </inertial>
+  </link>
+  <joint name="joint_imu" type="fixed">
+    <origin xyz="0 -0.02341 0.04927" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="imu_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  <link name="head_Link">
+    <inertial>
+      <origin xyz="0.33989 -0.000168 0.12029" rpy="0 0 0" />
+      <mass value="3.6885" />
+      <inertia ixx="0.026455" ixy="3.15E-05" ixz="0.00166" iyy="0.029221" iyz="-3.27E-05" izz="0.010918" />
+    </inertial>
+    <collision>
+      <origin xyz="0.41 0 0.005" rpy="0 0 0" />
       <geometry>
         <box size="0.04 0.12 0.14" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="joint_head"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="head_Link" />
-    <axis
-      xyz="0 0 0" />
+  <joint name="joint_head" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="head_Link" />
+    <axis xyz="0 0 0" />
   </joint>
-  <link
-    name="tail_link">
+  <link name="tail_link">
     <inertial>
-      <origin
-        xyz="-0.37448 0.000488 0.006495"
-        rpy="0 0 0" />
-      <mass
-        value="0.795" />
-      <inertia
-        ixx="0.0028118"
-        ixy="1.95E-05"
-        ixz="-7.87E-06"
-        iyy="0.0047506"
-        iyz="-1.57E-05"
-        izz="0.0029154" />
+      <origin xyz="-0.37448 0.000488 0.006495" rpy="0 0 0" />
+      <mass value="0.795" />
+      <inertia ixx="0.0028118" ixy="1.95E-05" ixz="-7.87E-06" iyy="0.0047506" iyz="-1.57E-05" izz="0.0029154" />
     </inertial>
     <collision>
-      <origin
-        xyz="-0.405 0 0.005"
-        rpy="0 0 0" />
+      <origin xyz="-0.405 0 0.005" rpy="0 0 0" />
       <geometry>
         <box size="0.025 0.12 0.14" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="joint_tail"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="tail_link" />
-    <axis
-      xyz="0 0 0" />
+  <joint name="joint_tail" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="tail_link" />
+    <axis xyz="0 0 0" />
   </joint>
-  <link
-    name="FL_hip">
+  <link name="FL_hip">
     <inertial>
-      <origin
-        xyz="-0.003841 -0.009068 0"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0033188"
-        ixy="7.16E-05"
-        ixz="-3.77E-07"
-        iyy="0.0048743"
-        iyz="4E-09"
-        izz="0.0037087" />
+      <origin xyz="-0.003841 -0.009068 0" rpy="0 0 0" />
+      <mass value="2.673" />
+      <inertia ixx="0.0033188" ixy="7.16E-05" ixz="-3.77E-07" iyy="0.0048743" iyz="4E-09" izz="0.0037087" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FL_hip.dae" />
+        <mesh filename="package://b2w_description/meshes/FL_hip.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0.12 0"
-        rpy="1.5707963267948966 0 0" />
+      <origin xyz="0 0.12 0" rpy="1.5707963267948966 0 0" />
       <geometry>
         <cylinder length="0.05" radius="0.07" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FL_hip_joint"
-    type="revolute">
-    <origin
-      xyz="0.3285 0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="FL_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
+  <joint name="FL_hip_joint" type="revolute">
+    <origin xyz="0.3285 0.072 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="FL_hip" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.87" upper="0.87" effort="200" velocity="23" />
   </joint>
-  <link
-    name="FL_thigh">
+  <link name="FL_thigh">
     <inertial>
-      <origin
-        xyz="-0.006279 -0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.062299"
-        ixy="0.00087781"
-        ixz="-0.0036475"
-        iyy="0.061399"
-        iyz="0.0083537"
-        izz="0.0081997" />
+      <origin xyz="-0.006279 -0.032049 -0.057835" rpy="0 0 0" />
+      <mass value="4.536" />
+      <inertia ixx="0.062299" ixy="0.00087781" ixz="-0.0036475" iyy="0.061399" iyz="0.0083537" izz="0.0081997" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FL_thigh.dae" />
+        <mesh filename="package://b2w_description/meshes/FL_thigh.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="-0.025 0 -0.16"
-        rpy="0 1.57 0" />
+      <origin xyz="-0.025 0 -0.16" rpy="0 1.57 0" />
       <geometry>
         <box size="0.32 0.0445 0.054" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FL_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="FL_hip" />
-    <child
-      link="FL_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
+  <joint name="FL_thigh_joint" type="revolute">
+    <origin xyz="0 0.11973 0" rpy="0 0 0" />
+    <parent link="FL_hip" />
+    <child link="FL_thigh" />
+    <axis xyz="0 1 0" />
+    <limit lower="-0.94" upper="4.69" effort="200" velocity="23" />
   </joint>
-  <link
-    name="FL_calf">
+  <link name="FL_calf">
     <inertial>
-      <origin
-        xyz="0.005278 0.017301 -0.29778"
-        rpy="0 0 0" />
-      <mass
-        value="2.2906" />
-      <inertia
-        ixx="0.029283"
-        ixy="0.000221"
-        ixz="-0.001064"
-        iyy="0.030868"
-        iyz="0.002093"
-        izz="0.00326" />
+      <origin xyz="0.005278 0.017301 -0.29778" rpy="0 0 0" />
+      <mass value="2.2906" />
+      <inertia ixx="0.029283" ixy="0.000221" ixz="-0.001064" iyy="0.030868" iyz="0.002093" izz="0.00326" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FL_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/FL_calf.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FL_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/FL_calf.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FL_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 -8.8203E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FL_thigh" />
-    <child
-      link="FL_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
+  <joint name="FL_calf_joint" type="revolute">
+    <origin xyz="0 -8.8203E-05 -0.35" rpy="0 0 0" />
+    <parent link="FL_thigh" />
+    <child link="FL_calf" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.82" upper="-0.43" effort="320" velocity="14" />
   </joint>
-  <link
-    name="FL_foot">
+  <link name="FL_foot">
     <inertial>
-      <origin
-        xyz="-0.000991 0.051053 -0.00113"
-        rpy="0 0 0" />
-      <mass
-        value="1.083" />
-      <inertia
-        ixx="0.004121"
-        ixy="6E-06"
-        ixz="-4.8E-05"
-        iyy="0.007776"
-        iyz="7E-06"
-        izz="0.004108" />
+      <origin xyz="-0.000991 0.051053 -0.00113" rpy="0 0 0" />
+      <mass value="1.083" />
+      <inertia ixx="0.004121" ixy="6E-06" ixz="-4.8E-05" iyy="0.007776" iyz="7E-06" izz="0.004108" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FL_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/FL_foot.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FL_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/FL_foot.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FL_foot_joint"
-    type="continuous">
-    <origin
-      xyz="0 0 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FL_calf" />
-    <child
-      link="FL_foot" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      effort="20"
-      velocity="50" />
+  <joint name="FL_foot_joint" type="continuous">
+    <origin xyz="0 0 -0.35" rpy="0 0 0" />
+    <parent link="FL_calf" />
+    <child link="FL_foot" />
+    <axis xyz="0 1 0" />
+    <limit effort="20" velocity="50" />
   </joint>
-  <link
-    name="FR_hip">
+  <link name="FR_hip">
     <inertial>
-      <origin
-        xyz="-0.003841 0.009068 0"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.003318772"
-        ixy="-7.16E-05"
-        ixz="-3.77E-07"
-        iyy="0.004874327"
-        iyz="-4E-09"
-        izz="0.003708731" />
+      <origin xyz="-0.003841 0.009068 0" rpy="0 0 0" />
+      <mass value="2.673" />
+      <inertia ixx="0.003318772" ixy="-7.16E-05" ixz="-3.77E-07" iyy="0.004874327" iyz="-4E-09" izz="0.003708731" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FR_hip.dae" />
+        <mesh filename="package://b2w_description/meshes/FR_hip.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 -0.12 0"
-        rpy="1.5707963267948966 0 0" />
+      <origin xyz="0 -0.12 0" rpy="1.5707963267948966 0 0" />
       <geometry>
         <cylinder length="0.05" radius="0.07" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FR_hip_joint"
-    type="revolute">
-    <origin
-      xyz="0.3285 -0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="FR_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
+  <joint name="FR_hip_joint" type="revolute">
+    <origin xyz="0.3285 -0.072 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="FR_hip" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.87" upper="0.87" effort="200" velocity="23" />
   </joint>
-  <link
-    name="FR_thigh">
+  <link name="FR_thigh">
     <inertial>
-      <origin
-        xyz="-0.006279 0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.06229908"
-        ixy="-0.000877808"
-        ixz="-0.003647464"
-        iyy="0.06139927"
-        iyz="-0.008353671"
-        izz="0.008199745" />
+      <origin xyz="-0.006279 0.032049 -0.057835" rpy="0 0 0" />
+      <mass value="4.536" />
+      <inertia ixx="0.06229908" ixy="-0.000877808" ixz="-0.003647464" iyy="0.06139927" iyz="-0.008353671" izz="0.008199745" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FR_thigh.dae" />
+        <mesh filename="package://b2w_description/meshes/FR_thigh.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
@@ -571,223 +507,113 @@
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FR_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 -0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="FR_hip" />
-    <child
-      link="FR_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
+  <joint name="FR_thigh_joint" type="revolute">
+    <origin xyz="0 -0.11973 0" rpy="0 0 0" />
+    <parent link="FR_hip" />
+    <child link="FR_thigh" />
+    <axis xyz="0 1 0" />
+    <limit lower="-0.94" upper="4.69" effort="200" velocity="23" />
   </joint>
-  <link
-    name="FR_calf">
+  <link name="FR_calf">
     <inertial>
-      <origin
-        xyz="0.005278 -0.017301 -0.297775"
-        rpy="0 0 0" />
-      <mass
-        value="2.290557" />
-      <inertia
-        ixx="0.029283"
-        ixy="-0.000221"
-        ixz="-0.001064"
-        iyy="0.030868"
-        iyz="-0.002093"
-        izz="0.00326" />
+      <origin xyz="0.005278 -0.017301 -0.297775" rpy="0 0 0" />
+      <mass value="2.290557" />
+      <inertia ixx="0.029283" ixy="-0.000221" ixz="-0.001064" iyy="0.030868" iyz="-0.002093" izz="0.00326" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FR_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/FR_calf.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FR_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/FR_calf.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FR_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 8.8205E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FR_thigh" />
-    <child
-      link="FR_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
+  <joint name="FR_calf_joint" type="revolute">
+    <origin xyz="0 8.8205E-05 -0.35" rpy="0 0 0" />
+    <parent link="FR_thigh" />
+    <child link="FR_calf" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.82" upper="-0.43" effort="320" velocity="14" />
   </joint>
-  <link
-    name="FR_foot">
+  <link name="FR_foot">
     <inertial>
-      <origin
-        xyz="-0.000991 -0.051053 -0.00113"
-        rpy="0 0 0" />
-      <mass
-        value="1.083" />
-      <inertia
-        ixx="0.004121"
-        ixy="-6E-06"
-        ixz="-4.8E-05"
-        iyy="0.007776"
-        iyz="-7E-06"
-        izz="0.004108" />
+      <origin xyz="-0.000991 -0.051053 -0.00113" rpy="0 0 0" />
+      <mass value="1.083" />
+      <inertia ixx="0.004121" ixy="-6E-06" ixz="-4.8E-05" iyy="0.007776" iyz="-7E-06" izz="0.004108" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FR_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/FR_foot.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/FR_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/FR_foot.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="FR_foot_joint"
-    type="continuous">
-    <origin
-      xyz="0 -0.001 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="FR_calf" />
-    <child
-      link="FR_foot" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      effort="20"
-      velocity="50" />
+  <joint name="FR_foot_joint" type="continuous">
+    <origin xyz="0 -0.001 -0.35" rpy="0 0 0" />
+    <parent link="FR_calf" />
+    <child link="FR_foot" />
+    <axis xyz="0 1 0" />
+    <limit effort="20" velocity="50" />
   </joint>
-  <link
-    name="RL_hip">
+  <link name="RL_hip">
     <inertial>
-      <origin
-        xyz="0.003841 -0.009068 0"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0033188"
-        ixy="-7.16E-05"
-        ixz="3.77E-07"
-        iyy="0.0048743"
-        iyz="4E-09"
-        izz="0.0037087" />
+      <origin xyz="0.003841 -0.009068 0" rpy="0 0 0" />
+      <mass value="2.673" />
+      <inertia ixx="0.0033188" ixy="-7.16E-05" ixz="3.77E-07" iyy="0.0048743" iyz="4E-09" izz="0.0037087" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RL_hip.dae" />
+        <mesh filename="package://b2w_description/meshes/RL_hip.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0.12 0"
-        rpy="1.5707963267948966 0 0" />
+      <origin xyz="0 0.12 0" rpy="1.5707963267948966 0 0" />
       <geometry>
         <cylinder length="0.05" radius="0.07" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RL_hip_joint"
-    type="revolute">
-    <origin
-      xyz="-0.3285 0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="RL_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
+  <joint name="RL_hip_joint" type="revolute">
+    <origin xyz="-0.3285 0.072 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="RL_hip" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.87" upper="0.87" effort="200" velocity="23" />
   </joint>
-  <link
-    name="RL_thigh">
+  <link name="RL_thigh">
     <inertial>
-      <origin
-        xyz="-0.006279 -0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.06229908"
-        ixy="0.000877808"
-        ixz="-0.003647464"
-        iyy="0.06139927"
-        iyz="0.008353671"
-        izz="0.008199745" />
+      <origin xyz="-0.006279 -0.032049 -0.057835" rpy="0 0 0" />
+      <mass value="4.536" />
+      <inertia ixx="0.06229908" ixy="0.000877808" ixz="-0.003647464" iyy="0.06139927" iyz="0.008353671" izz="0.008199745" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RL_thigh.dae" />
+        <mesh filename="package://b2w_description/meshes/RL_thigh.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
@@ -797,223 +623,113 @@
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RL_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="RL_hip" />
-    <child
-      link="RL_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
+  <joint name="RL_thigh_joint" type="revolute">
+    <origin xyz="0 0.11973 0" rpy="0 0 0" />
+    <parent link="RL_hip" />
+    <child link="RL_thigh" />
+    <axis xyz="0 1 0" />
+    <limit lower="-0.94" upper="4.69" effort="200" velocity="23" />
   </joint>
-  <link
-    name="RL_calf">
+  <link name="RL_calf">
     <inertial>
-      <origin
-        xyz="0.005278 0.017301 -0.29778"
-        rpy="0 0 0" />
-      <mass
-        value="2.2906" />
-      <inertia
-        ixx="0.029283"
-        ixy="0.000221"
-        ixz="-0.001064"
-        iyy="0.030868"
-        iyz="0.002093"
-        izz="0.00326" />
+      <origin xyz="0.005278 0.017301 -0.29778" rpy="0 0 0" />
+      <mass value="2.2906" />
+      <inertia ixx="0.029283" ixy="0.000221" ixz="-0.001064" iyy="0.030868" iyz="0.002093" izz="0.00326" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RL_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/RL_calf.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RL_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/RL_calf.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RL_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 -8.8205E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RL_thigh" />
-    <child
-      link="RL_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
+  <joint name="RL_calf_joint" type="revolute">
+    <origin xyz="0 -8.8205E-05 -0.35" rpy="0 0 0" />
+    <parent link="RL_thigh" />
+    <child link="RL_calf" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.82" upper="-0.43" effort="320" velocity="14" />
   </joint>
-  <link
-    name="RL_foot">
+  <link name="RL_foot">
     <inertial>
-      <origin
-        xyz="-0.000991 0.051053 -0.00113"
-        rpy="0 0 0" />
-      <mass
-        value="1.083" />
-      <inertia
-        ixx="0.004121"
-        ixy="6E-06"
-        ixz="-4.8E-05"
-        iyy="0.007776"
-        iyz="7E-06"
-        izz="0.004108" />
+      <origin xyz="-0.000991 0.051053 -0.00113" rpy="0 0 0" />
+      <mass value="1.083" />
+      <inertia ixx="0.004121" ixy="6E-06" ixz="-4.8E-05" iyy="0.007776" iyz="7E-06" izz="0.004108" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RL_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/RL_foot.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RL_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/RL_foot.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RL_foot_joint"
-    type="continuous">
-    <origin
-      xyz="0 0 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RL_calf" />
-    <child
-      link="RL_foot" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      effort="20"
-      velocity="50" />
+  <joint name="RL_foot_joint" type="continuous">
+    <origin xyz="0 0 -0.35" rpy="0 0 0" />
+    <parent link="RL_calf" />
+    <child link="RL_foot" />
+    <axis xyz="0 1 0" />
+    <limit effort="20" velocity="50" />
   </joint>
-  <link
-    name="RR_hip">
+  <link name="RR_hip">
     <inertial>
-      <origin
-        xyz="0.003841 0.009068 0"
-        rpy="0 0 0" />
-      <mass
-        value="2.673" />
-      <inertia
-        ixx="0.0033188"
-        ixy="7.16E-05"
-        ixz="3.77E-07"
-        iyy="0.0048743"
-        iyz="-4E-09"
-        izz="0.0037087" />
+      <origin xyz="0.003841 0.009068 0" rpy="0 0 0" />
+      <mass value="2.673" />
+      <inertia ixx="0.0033188" ixy="7.16E-05" ixz="3.77E-07" iyy="0.0048743" iyz="-4E-09" izz="0.0037087" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RR_hip.dae" />
+        <mesh filename="package://b2w_description/meshes/RR_hip.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 -0.12 0"
-        rpy="1.5707963267948966 0 0" />
+      <origin xyz="0 -0.12 0" rpy="1.5707963267948966 0 0" />
       <geometry>
         <cylinder length="0.05" radius="0.07" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RR_hip_joint"
-    type="revolute">
-    <origin
-      xyz="-0.3285 -0.072 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="RR_hip" />
-    <axis
-      xyz="1 0 0" />
-    <limit
-      lower="-0.87"
-      upper="0.87"
-      effort="200"
-      velocity="23" />
+  <joint name="RR_hip_joint" type="revolute">
+    <origin xyz="-0.3285 -0.072 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="RR_hip" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.87" upper="0.87" effort="200" velocity="23" />
   </joint>
-  <link
-    name="RR_thigh">
+  <link name="RR_thigh">
     <inertial>
-      <origin
-        xyz="-0.006279 0.032049 -0.057835"
-        rpy="0 0 0" />
-      <mass
-        value="4.536" />
-      <inertia
-        ixx="0.062299"
-        ixy="-0.00087781"
-        ixz="-0.0036475"
-        iyy="0.061399"
-        iyz="-0.0083537"
-        izz="0.0081997" />
+      <origin xyz="-0.006279 0.032049 -0.057835" rpy="0 0 0" />
+      <mass value="4.536" />
+      <inertia ixx="0.062299" ixy="-0.00087781" ixz="-0.0036475" iyy="0.061399" iyz="-0.0083537" izz="0.0081997" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RR_thigh.dae" />
+        <mesh filename="package://b2w_description/meshes/RR_thigh.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
@@ -1023,226 +739,272 @@
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RR_thigh_joint"
-    type="revolute">
-    <origin
-      xyz="0 -0.11973 0"
-      rpy="0 0 0" />
-    <parent
-      link="RR_hip" />
-    <child
-      link="RR_thigh" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-0.94"
-      upper="4.69"
-      effort="200"
-      velocity="23" />
+  <joint name="RR_thigh_joint" type="revolute">
+    <origin xyz="0 -0.11973 0" rpy="0 0 0" />
+    <parent link="RR_hip" />
+    <child link="RR_thigh" />
+    <axis xyz="0 1 0" />
+    <limit lower="-0.94" upper="4.69" effort="200" velocity="23" />
   </joint>
-  <link
-    name="RR_calf">
+  <link name="RR_calf">
     <inertial>
-      <origin
-        xyz="0.005278 -0.017301 -0.29778"
-        rpy="0 0 0" />
-      <mass
-        value="2.2906" />
-      <inertia
-        ixx="0.029283"
-        ixy="-0.000221"
-        ixz="-0.001064"
-        iyy="0.030868"
-        iyz="-0.002093"
-        izz="0.00326" />
+      <origin xyz="0.005278 -0.017301 -0.29778" rpy="0 0 0" />
+      <mass value="2.2906" />
+      <inertia ixx="0.029283" ixy="-0.000221" ixz="-0.001064" iyy="0.030868" iyz="-0.002093" izz="0.00326" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RR_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/RR_calf.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RR_calf.dae" />
+        <mesh filename="package://b2w_description/meshes/RR_calf.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RR_calf_joint"
-    type="revolute">
-    <origin
-      xyz="0 8.8205E-05 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RR_thigh" />
-    <child
-      link="RR_calf" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-2.82"
-      upper="-0.43"
-      effort="320"
-      velocity="14" />
+  <joint name="RR_calf_joint" type="revolute">
+    <origin xyz="0 8.8205E-05 -0.35" rpy="0 0 0" />
+    <parent link="RR_thigh" />
+    <child link="RR_calf" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.82" upper="-0.43" effort="320" velocity="14" />
   </joint>
-  <link
-    name="RR_foot">
+  <link name="RR_foot">
     <inertial>
-      <origin
-        xyz="-0.000991 -0.051053 -0.00113"
-        rpy="0 0 0" />
-      <mass
-        value="1.083" />
-      <inertia
-        ixx="0.004121"
-        ixy="-6E-06"
-        ixz="-4.8E-05"
-        iyy="0.007776"
-        iyz="-7E-06"
-        izz="0.004108" />
+      <origin xyz="-0.000991 -0.051053 -0.00113" rpy="0 0 0" />
+      <mass value="1.083" />
+      <inertia ixx="0.004121" ixy="-6E-06" ixz="-4.8E-05" iyy="0.007776" iyz="-7E-06" izz="0.004108" />
     </inertial>
     <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RR_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/RR_foot.dae" />
       </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
+      <material name="">
+        <color rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh
-          filename="package://b2w_description/meshes/RR_foot.dae" />
+        <mesh filename="package://b2w_description/meshes/RR_foot.dae" />
       </geometry>
     </collision>
   </link>
-  <joint
-    name="RR_foot_joint"
-    type="continuous">
-    <origin
-      xyz="0 -0.001 -0.35"
-      rpy="0 0 0" />
-    <parent
-      link="RR_calf" />
-    <child
-      link="RR_foot" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      effort="20"
-      velocity="50" />
+  <joint name="RR_foot_joint" type="continuous">
+    <origin xyz="0 -0.001 -0.35" rpy="0 0 0" />
+    <parent link="RR_calf" />
+    <child link="RR_foot" />
+    <axis xyz="0 1 0" />
+    <limit effort="20" velocity="50" />
   </joint>
-  <link
-    name="f_oc_link">
+  <link name="f_oc_link">
     <inertial>
-      <origin
-        xyz="-3.39E-15 1.46E-16 -6.11E-16"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="-3.89E-44"
-        ixz="5.04E-44"
-        iyy="1.68E-27"
-        iyz="2.81E-45"
-        izz="1.68E-27" />
+      <origin xyz="-3.39E-15 1.46E-16 -6.11E-16" rpy="0 0 0" />
+      <mass value="4.19E-15" />
+      <inertia ixx="1.68E-27" ixy="-3.89E-44" ixz="5.04E-44" iyy="1.68E-27" iyz="2.81E-45" izz="1.68E-27" />
     </inertial>
   </link>
-  <joint
-    name="f_oc_link"
-    type="fixed">
-    <origin
-      xyz="0.3993 0 -0.01576"
-      rpy="-1.5708 0 -1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="f_oc_link" />
-    <axis
-      xyz="0 0 0" />
+  <joint name="f_oc_link" type="fixed">
+    <origin xyz="0.3993 0 -0.01576" rpy="-1.5708 0 -1.5708" />
+    <parent link="trunk" />
+    <child link="f_oc_link" />
+    <axis xyz="0 0 0" />
   </joint>
-  <link
-    name="logo_link">
+  <link name="logo_link">
     <inertial>
-      <origin
-        xyz="0 -2.3E-23 0"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-15" />
-      <inertia
-        ixx="1.68E-27"
-        ixy="0"
-        ixz="0"
-        iyy="1.68E-27"
-        iyz="0"
-        izz="1.68E-27" />
+      <origin xyz="0 -2.3E-23 0" rpy="0 0 0" />
+      <mass value="4.19E-15" />
+      <inertia ixx="1.68E-27" ixy="0" ixz="0" iyy="1.68E-27" iyz="0" izz="1.68E-27" />
     </inertial>
   </link>
-  <joint
-    name="joint_logo"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="logo_link" />
-    <axis
-      xyz="0 0 0" />
+  <joint name="joint_logo" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="trunk" />
+    <child link="logo_link" />
+    <axis xyz="0 0 0" />
   </joint>
-  <link
-    name="r_oc_ink">
+  <link name="r_oc_ink">
     <inertial>
-      <origin
-        xyz="-0.39143 1.52E-07 -0.011961"
-        rpy="0 0 0" />
-      <mass
-        value="4.19E-12" />
-      <inertia
-        ixx="1.68E-22"
-        ixy="9.03E-39"
-        ixz="0"
-        iyy="1.68E-22"
-        iyz="-1.2E-38"
-        izz="1.68E-22" />
+      <origin xyz="-0.39143 1.52E-07 -0.011961" rpy="0 0 0" />
+      <mass value="4.19E-12" />
+      <inertia ixx="1.68E-22" ixy="9.03E-39" ixz="0" iyy="1.68E-22" iyz="-1.2E-38" izz="1.68E-22" />
     </inertial>
   </link>
-  <joint
-    name="joint_r_oc"
-    type="fixed">
-    <origin
-      xyz="-0.39143 0 -0.026131"
-      rpy="-1.5708 0 1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="r_oc_ink" />
-    <axis
-      xyz="0 0 0" />
+  <joint name="joint_r_oc" type="fixed">
+    <origin xyz="-0.39143 0 -0.026131" rpy="-1.5708 0 1.5708" />
+    <parent link="trunk" />
+    <child link="r_oc_ink" />
+    <axis xyz="0 0 0" />
   </joint>
+
+  <transmission name="FL_hip_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_hip_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_hip_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FL_thigh_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_thigh_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_thigh_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FL_calf_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_calf_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_calf_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FL_foot_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_foot_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_foot_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="FR_hip_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_hip_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_hip_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FR_thigh_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_thigh_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_thigh_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FR_calf_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_calf_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_calf_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FR_foot_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_foot_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_foot_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="RL_hip_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RL_hip_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RL_hip_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="RL_thigh_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RL_thigh_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RL_thigh_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="RL_calf_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RL_calf_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RL_calf_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="RL_foot_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RL_foot_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RL_foot_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="RR_hip_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RR_hip_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RR_hip_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="RR_thigh_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RR_thigh_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RR_thigh_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="RR_calf_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RR_calf_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RR_calf_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="RR_foot_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="RR_foot_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="RR_foot_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
 </robot>