diff --git a/simulation/resources/spot.urdf b/simulation/resources/spot.urdf
index b4a5492..0e319f5 100644
--- a/simulation/resources/spot.urdf
+++ b/simulation/resources/spot.urdf
@@ -1,610 +1,583 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot name="rex" xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <material name="yellow">
-    <color rgba="0.92 0.83 0.0 1"/>
-  </material>
-  <material name="black">
-    <color rgba="0.1 0.1 0.1 1"/>
-  </material>
-  <material name="grey">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <!-- Params -->
-  <!-- Macros -->
-  <!-- Robot Body -->
-  <link name="base_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/mainbody.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="black"/>
-      <origin rpy="0 0 0" xyz="-0.045 -0.060 -0.015"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.14 0.11 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="1.20"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
+    <material name="white">
+        <color rgba="1 1 1 1" />
+    </material>
+    <material name="black">
+        <color rgba="0.1 0.1 0.1 1" />
+    </material>
+    <material name="grey">
+        <color rgba="0.6 0.6 0.6 1" />
+    </material>
+    <!-- Params -->
+    <!-- Macros -->
+    <!-- Robot Body -->
+    <link name="base_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/mainbody.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <material name="black" />
+            <origin rpy="0 0 0" xyz="-0.045 -0.060 -0.015" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.14 0.11 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="1.20" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
 
-<!--  @TODO add lidar sensor-->
-  <!-- Lidar Backpack -->
-<!--  <link name="lidar_link">-->
-<!--    <visual>-->
-<!--      <geometry>-->
-<!--        <mesh filename="stl/rplidar_main.STL" scale="0.001 0.001 0.001"/>-->
-<!--      </geometry>-->
-<!--      <material name="black"/>-->
-<!--      <origin rpy="1.57075 0 0" xyz="-0.06425 0.02885 0"/>-->
-<!--    </visual>-->
-<!--    <collision>-->
-<!--      <geometry>-->
-<!--        <box size="0.0985 0.0577 0.07"/>-->
-<!--      </geometry>-->
-<!--      <origin rpy="0 0 0" xyz="0 0 0"/>-->
-<!--    </collision>-->
-<!--    <inertial>-->
-<!--      <mass value="0.50"/>-->
-<!--      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>-->
-<!--    </inertial>-->
-<!--  </link>-->
-<!--  <joint name="base_lidar" type="fixed">-->
-<!--    <parent link="base_link"/>-->
-<!--    <child link="lidar_link"/>-->
-<!--    <origin xyz="0 0 0.035"/>-->
-<!--  </joint>-->
+    <!-- create head & tail -->
+    <link name="chassis_front_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/frontpart.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.040 -0.060 -0.0140" />
+            <material name="white" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.058 0.11 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.145 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.05" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <joint name="chassis_base_front" type="fixed">
+        <parent link="base_link" />
+        <child link="chassis_front_link" />
+    </joint>
+    <link name="chassis_rear_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/backpart.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.040 -0.060 -0.0140" />
+            <material name="white" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.04 0.11 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="0.135 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.05" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <joint name="chassis_base_rear" type="fixed">
+        <parent link="base_link" />
+        <child link="chassis_rear_link" />
+    </joint>
 
-  <!-- create head & tail -->
-  <link name="chassis_front_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/frontpart.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.040 -0.060 -0.0140"/>
-      <material name="yellow"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.058 0.11 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.145 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="0.05"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <joint name="chassis_base_front" type="fixed">
-    <parent link="base_link"/>
-    <child link="chassis_front_link"/>
-  </joint>
-  <link name="chassis_rear_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/backpart.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.040 -0.060 -0.0140"/>
-      <material name="yellow"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.04 0.11 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0.135 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="0.05"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <joint name="chassis_base_rear" type="fixed">
-    <parent link="base_link"/>
-    <child link="chassis_rear_link"/>
-  </joint>
+    <!-- create Legs -->
+    <link name="front_left_shoulder_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/lshoulder.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.135 -0.02 -0.01" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.044 0.038 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.10" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_left_leg_link_cover">
+        <visual>
+            <geometry>
+                <mesh filename="stl/larm_cover.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02" />
+            <material name="white" />
+        </visual>
+        <inertial>
+            <mass value="0.5" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_left_leg_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/larm.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+            <geometry>
+                <box size="0.028 0.036 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_left_foot_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/lfoot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.120 0.04 0.1" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.026 0.020 0.115" />
+            </geometry>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_left_toe_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <material name="grey" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <contact_coefficients mu="100.0" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <joint name="motor_front_left_shoulder" type="revolute">
+        <parent link="base_link" />
+        <child link="front_left_shoulder_link" />
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="-0.093 -0.036 0" />
+        <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="motor_front_left_leg" type="revolute">
+        <parent link="front_left_shoulder_link" />
+        <child link="front_left_leg_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0 -0.052 0" />
+        <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.0" />
+    </joint>
+    <joint name="front_left_leg_cover_joint" type="fixed">
+        <parent link="front_left_leg_link" />
+        <child link="front_left_leg_link_cover" />
+        <origin xyz="0 0 0" />
+    </joint>
+    <joint name="foot_motor_front_left" type="revolute">
+        <parent link="front_left_leg_link" />
+        <child link="front_left_foot_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.01 0 -0.12" />
+        <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="front_left_toe" type="fixed">
+        <parent link="front_left_foot_link" />
+        <child link="front_left_toe_link" />
+        <origin xyz="0 0 -0.115" />
+    </joint>
 
-  <!-- create Legs -->
-  <link name="front_left_shoulder_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/lshoulder.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.135 -0.02 -0.01"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.044 0.038 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="0.10"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_left_leg_link_cover">
-    <visual>
-      <geometry>
-        <mesh filename="stl/larm_cover.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02"/>
-      <material name="yellow"/>
-    </visual>
-    <inertial>
-      <mass value="0.5"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_left_leg_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/larm.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-      <geometry>
-        <box size="0.028 0.036 0.12"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.1"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_left_foot_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/lfoot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.120 0.04 0.1"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.026 0.020 0.115"/>
-      </geometry>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-    </collision>
-    <inertial>
-      <mass value="0.1"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_left_toe_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <material name="grey"/>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <contact_coefficients mu="100.0"/>
-    </collision>
-    <inertial>
-      <mass value="0.005"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <joint name="motor_front_left_shoulder" type="revolute">
-    <parent link="base_link"/>
-    <child link="front_left_shoulder_link"/>
-    <axis xyz="1 0 0"/>
-    <origin rpy="0 0 0" xyz="-0.093 -0.036 0"/>
-    <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="motor_front_left_leg" type="revolute">
-    <parent link="front_left_shoulder_link"/>
-    <child link="front_left_leg_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="0 -0.052 0"/>
-    <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.0"/>
-  </joint>
-  <joint name="front_left_leg_cover_joint" type="fixed">
-    <parent link="front_left_leg_link"/>
-    <child link="front_left_leg_link_cover"/>
-    <origin xyz="0 0 0"/>
-  </joint>
-  <joint name="foot_motor_front_left" type="revolute">
-    <parent link="front_left_leg_link"/>
-    <child link="front_left_foot_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="-0.01 0 -0.12"/>
-    <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="front_left_toe" type="fixed">
-    <parent link="front_left_foot_link"/>
-    <child link="front_left_toe_link"/>
-    <origin xyz="0 0 -0.115"/>
-  </joint>
+    <link name="front_right_shoulder_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rshoulder.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.135 -0.09 -0.01" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.044 0.038 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.10" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_right_leg_link_cover">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rarm_cover.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02" />
+            <material name="white" />
+        </visual>
+        <inertial>
+            <mass value="0.5" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_right_leg_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rarm.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+            <geometry>
+                <box size="0.028 0.036 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_right_foot_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rfoot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.120 -0.15 0.1" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.026 0.020 0.115" />
+            </geometry>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="front_right_toe_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <material name="grey" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <contact_coefficients mu="100.0" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <joint name="motor_front_right_shoulder" type="revolute">
+        <parent link="base_link" />
+        <child link="front_right_shoulder_link" />
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="-0.093 0.036 0" />
+        <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="motor_front_right_leg" type="revolute">
+        <parent link="front_right_shoulder_link" />
+        <child link="front_right_leg_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0 0.052 0" />
+        <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.0" />
+    </joint>
+    <joint name="front_right_leg_cover_joint" type="fixed">
+        <parent link="front_right_leg_link" />
+        <child link="front_right_leg_link_cover" />
+        <origin xyz="0 0 0" />
+    </joint>
+    <joint name="foot_motor_front_right" type="revolute">
+        <parent link="front_right_leg_link" />
+        <child link="front_right_foot_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.01 0 -0.12" />
+        <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="front_right_toe" type="fixed">
+        <parent link="front_right_foot_link" />
+        <child link="front_right_toe_link" />
+        <origin xyz="0 0 -0.115" />
+    </joint>
 
-  <link name="front_right_shoulder_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rshoulder.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.135 -0.09 -0.01"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.044 0.038 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="0.10"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_right_leg_link_cover">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rarm_cover.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02"/>
-      <material name="yellow"/>
-    </visual>
-    <inertial>
-      <mass value="0.5"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_right_leg_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rarm.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-      <geometry>
-        <box size="0.028 0.036 0.12"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.1"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_right_foot_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rfoot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.120 -0.15 0.1"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.026 0.020 0.115"/>
-      </geometry>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-    </collision>
-    <inertial>
-      <mass value="0.1"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="front_right_toe_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <material name="grey"/>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <contact_coefficients mu="100.0"/>
-    </collision>
-    <inertial>
-      <mass value="0.005"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <joint name="motor_front_right_shoulder" type="revolute">
-    <parent link="base_link"/>
-    <child link="front_right_shoulder_link"/>
-    <axis xyz="1 0 0"/>
-    <origin rpy="0 0 0" xyz="-0.093 0.036 0"/>
-    <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="motor_front_right_leg" type="revolute">
-    <parent link="front_right_shoulder_link"/>
-    <child link="front_right_leg_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="0 0.052 0"/>
-    <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.0"/>
-  </joint>
-  <joint name="front_right_leg_cover_joint" type="fixed">
-    <parent link="front_right_leg_link"/>
-    <child link="front_right_leg_link_cover"/>
-    <origin xyz="0 0 0"/>
-  </joint>
-  <joint name="foot_motor_front_right" type="revolute">
-    <parent link="front_right_leg_link"/>
-    <child link="front_right_foot_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="-0.01 0 -0.12"/>
-    <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="front_right_toe" type="fixed">
-    <parent link="front_right_foot_link"/>
-    <child link="front_right_toe_link"/>
-    <origin xyz="0 0 -0.115"/>
-  </joint>
+    <link name="rear_left_shoulder_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/lshoulder.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.135 -0.02 -0.01" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.044 0.038 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.10" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_left_leg_link_cover">
+        <visual>
+            <geometry>
+                <mesh filename="stl/larm_cover.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02" />
+            <material name="white" />
+        </visual>
+        <inertial>
+            <mass value="0.5" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_left_leg_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/larm.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+            <geometry>
+                <box size="0.028 0.036 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.10" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_left_foot_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/lfoot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.120 0.04 0.1" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.026 0.020 0.115" />
+            </geometry>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_left_toe_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <material name="grey" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <contact_coefficients mu="100.0" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <joint name="motor_rear_left_shoulder" type="revolute">
+        <parent link="base_link" />
+        <child link="rear_left_shoulder_link" />
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="0.093 -0.036 0" />
+        <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="motor_rear_left_leg" type="revolute">
+        <parent link="rear_left_shoulder_link" />
+        <child link="rear_left_leg_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0 -0.052 0" />
+        <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.0" />
+    </joint>
+    <joint name="rear_left_leg_cover_joint" type="fixed">
+        <parent link="rear_left_leg_link" />
+        <child link="rear_left_leg_link_cover" />
+        <origin xyz="0 0 0" />
+    </joint>
+    <joint name="foot_motor_rear_left" type="revolute">
+        <parent link="rear_left_leg_link" />
+        <child link="rear_left_foot_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.01 0 -0.12" />
+        <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="rear_left_toe" type="fixed">
+        <parent link="rear_left_foot_link" />
+        <child link="rear_left_toe_link" />
+        <origin xyz="0 0 -0.115" />
+    </joint>
 
-  <link name="rear_left_shoulder_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/lshoulder.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.135 -0.02 -0.01"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.044 0.038 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="0.10"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_left_leg_link_cover">
-    <visual>
-      <geometry>
-        <mesh filename="stl/larm_cover.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02"/>
-      <material name="yellow"/>
-    </visual>
-    <inertial>
-      <mass value="0.5"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_left_leg_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/larm.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 0.04 -0.02"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-      <geometry>
-        <box size="0.028 0.036 0.12"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.10"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_left_foot_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/lfoot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.120 0.04 0.1"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.026 0.020 0.115"/>
-      </geometry>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-    </collision>
-    <inertial>
-      <mass value="0.1"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_left_toe_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <material name="grey"/>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <contact_coefficients mu="100.0"/>
-    </collision>
-    <inertial>
-      <mass value="0.005"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <joint name="motor_rear_left_shoulder" type="revolute">
-    <parent link="base_link"/>
-    <child link="rear_left_shoulder_link"/>
-    <axis xyz="1 0 0"/>
-    <origin rpy="0 0 0" xyz="0.093 -0.036 0"/>
-    <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="motor_rear_left_leg" type="revolute">
-    <parent link="rear_left_shoulder_link"/>
-    <child link="rear_left_leg_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="0 -0.052 0"/>
-    <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.0"/>
-  </joint>
-  <joint name="rear_left_leg_cover_joint" type="fixed">
-    <parent link="rear_left_leg_link"/>
-    <child link="rear_left_leg_link_cover"/>
-    <origin xyz="0 0 0"/>
-  </joint>
-  <joint name="foot_motor_rear_left" type="revolute">
-    <parent link="rear_left_leg_link"/>
-    <child link="rear_left_foot_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="-0.01 0 -0.12"/>
-    <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="rear_left_toe" type="fixed">
-    <parent link="rear_left_foot_link"/>
-    <child link="rear_left_toe_link"/>
-    <origin xyz="0 0 -0.115"/>
-  </joint>
-
-  <link name="rear_right_shoulder_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rshoulder.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.135 -0.09 -0.01"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.044 0.038 0.07"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-    <inertial>
-      <mass value="0.10"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_right_leg_link_cover">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rarm_cover.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02"/>
-      <material name="yellow"/>
-    </visual>
-    <inertial>
-      <mass value="0.5"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_right_leg_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rarm.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-      <geometry>
-        <box size="0.028 0.036 0.12"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.10"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_right_foot_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/rfoot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="-0.120 -0.15 0.1"/>
-      <material name="black"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.026 0.020 0.115"/>
-      </geometry>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-    </collision>
-    <inertial>
-      <mass value="0.1"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <link name="rear_right_toe_link">
-    <visual>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <material name="grey"/>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <origin rpy="0 -0.40010 0" xyz="0 -0.01 0"/>
-      <contact_coefficients mu="100.0"/>
-    </collision>
-    <inertial>
-      <mass value="0.005"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
-    </inertial>
-  </link>
-  <joint name="motor_rear_right_shoulder" type="revolute">
-    <parent link="base_link"/>
-    <child link="rear_right_shoulder_link"/>
-    <axis xyz="1 0 0"/>
-    <origin rpy="0 0 0" xyz="0.093 0.036 0"/>
-    <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="motor_rear_right_leg" type="revolute">
-    <parent link="rear_right_shoulder_link"/>
-    <child link="rear_right_leg_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="0 0.052 0"/>
-    <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.0"/>
-  </joint>
-  <joint name="rear_right_leg_cover_joint" type="fixed">
-    <parent link="rear_right_leg_link"/>
-    <child link="rear_right_leg_link_cover"/>
-    <origin xyz="0 0 0"/>
-  </joint>
-  <joint name="foot_motor_rear_right" type="revolute">
-    <parent link="rear_right_leg_link"/>
-    <child link="rear_right_foot_link"/>
-    <axis xyz="0 1 0"/>
-    <origin rpy="0 0 0" xyz="-0.01 0 -0.12"/>
-    <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5"/>
-    <dynamics damping="0.0" friction="0.5"/>
-  </joint>
-  <joint name="rear_right_toe" type="fixed">
-    <parent link="rear_right_foot_link"/>
-    <child link="rear_right_toe_link"/>
-    <origin xyz="0 0 -0.115"/>
-  </joint>
+    <link name="rear_right_shoulder_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rshoulder.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.135 -0.09 -0.01" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.044 0.038 0.07" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="0 0 0" />
+        </collision>
+        <inertial>
+            <mass value="0.10" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_right_leg_link_cover">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rarm_cover.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02" />
+            <material name="white" />
+        </visual>
+        <inertial>
+            <mass value="0.5" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_right_leg_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rarm.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.125 -0.15 -0.02" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+            <geometry>
+                <box size="0.028 0.036 0.12" />
+            </geometry>
+        </collision>
+        <inertial>
+            <mass value="0.10" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_right_foot_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/rfoot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 0 0" xyz="-0.120 -0.15 0.1" />
+            <material name="black" />
+        </visual>
+        <collision>
+            <geometry>
+                <box size="0.026 0.020 0.115" />
+            </geometry>
+            <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050" />
+        </collision>
+        <inertial>
+            <mass value="0.1" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <link name="rear_right_toe_link">
+        <visual>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <material name="grey" />
+        </visual>
+        <collision>
+            <geometry>
+                <mesh filename="stl/foot.stl" scale="0.001 0.001 0.001" />
+            </geometry>
+            <origin rpy="0 -0.40010 0" xyz="0 -0.01 0" />
+            <contact_coefficients mu="100.0" />
+        </collision>
+        <inertial>
+            <mass value="0.005" />
+            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+        </inertial>
+    </link>
+    <joint name="motor_rear_right_shoulder" type="revolute">
+        <parent link="base_link" />
+        <child link="rear_right_shoulder_link" />
+        <axis xyz="1 0 0" />
+        <origin rpy="0 0 0" xyz="0.093 0.036 0" />
+        <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.7" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="motor_rear_right_leg" type="revolute">
+        <parent link="rear_right_shoulder_link" />
+        <child link="rear_right_leg_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="0 0.052 0" />
+        <limit effort="100.0" lower="-2.17" upper="0.97" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.0" />
+    </joint>
+    <joint name="rear_right_leg_cover_joint" type="fixed">
+        <parent link="rear_right_leg_link" />
+        <child link="rear_right_leg_link_cover" />
+        <origin xyz="0 0 0" />
+    </joint>
+    <joint name="foot_motor_rear_right" type="revolute">
+        <parent link="rear_right_leg_link" />
+        <child link="rear_right_foot_link" />
+        <axis xyz="0 1 0" />
+        <origin rpy="0 0 0" xyz="-0.01 0 -0.12" />
+        <limit effort="100.0" lower="-0.1" upper="2.59" velocity="0.5" />
+        <dynamics damping="0.0" friction="0.5" />
+    </joint>
+    <joint name="rear_right_toe" type="fixed">
+        <parent link="rear_right_foot_link" />
+        <child link="rear_right_toe_link" />
+        <origin xyz="0 0 -0.115" />
+    </joint>
 </robot>
\ No newline at end of file