From e36eae76ebe4df88993a1b850b75651ec1e398e2 Mon Sep 17 00:00:00 2001
From: Konstantinos Chatzilygeroudis <costashatz@gmail.com>
Date: Sun, 18 May 2014 16:07:48 +0300
Subject: [PATCH] Changed URDF structure..

---
 .../urdf/modules/arms.xacro                   | 106 +++++
 .../urdf/modules/cameras.xacro                |  16 +
 .../urdf/modules/hands.xacro                  |  97 ++++
 .../urdf/modules/head.xacro                   |  37 ++
 .../urdf/modules/imu.xacro                    |  13 +
 .../urdf/modules/legs.xacro                   | 199 ++++++++
 .../urdf/modules/sonars.xacro                 |  30 ++
 .../urdf/modules/torso.xacro                  |  19 +
 .../urdf/nao_dcm_H21.xacro                    |  11 +-
 .../urdf/nao_dcm_H21_structure.xacro          | 419 ----------------
 .../urdf/nao_dcm_H25.xacro                    |  11 +-
 .../urdf/nao_dcm_H25_structure.xacro          | 449 ------------------
 12 files changed, 537 insertions(+), 870 deletions(-)
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/arms.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/cameras.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/head.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/imu.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/legs.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/sonars.xacro
 create mode 100644 nao_dcm_common/nao_dcm_description/urdf/modules/torso.xacro
 delete mode 100644 nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro
 delete mode 100644 nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro

diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/arms.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/arms.xacro
new file mode 100644
index 0000000..03b8a02
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/arms.xacro
@@ -0,0 +1,106 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- ARM LINKS/JOINTS -->
+    <!-- Shoulder Roll and Elbow Roll Links are different for the 2 arms -->
+    <link name="LShoulderDummy">
+        <xacro:visuals_collisions_shoulder_roll side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
+            <mass value="0.15777"/>
+            <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
+        </inertial>
+    </link>
+    <link name="RShoulderDummy">
+        <xacro:visuals_collisions_shoulder_roll side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.02429 -0.00952 0.00320"/>
+            <mass value="0.15974"/>
+            <inertia ixx="0.00011012031" ixy="0.00007669131" ixz="-0.00002604607" iyy="0.00036757653" iyz="0.00001209828" izz="0.00035461772"/>
+        </inertial>
+    </link>
+    <link name="LElbowDummy">
+        <xacro:visuals_collisions_elbow_roll side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.02556 0.00281 0.00076"/>
+            <mass value="0.07761"/>
+            <inertia ixx="0.00002533220" ixy="-0.00000234271" ixz="0.00000007459" iyy="0.00008913220" iyz="-0.00000002655" izz="0.00008728726"/>
+        </inertial>
+    </link>
+    <link name="RElbowDummy">
+        <xacro:visuals_collisions_elbow_roll side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.02552 -0.00281 0.00090"/>
+            <mass value="0.07778"/>
+            <inertia ixx="0.00002539070" ixy="0.00000233243" ixz="-0.00000060117" iyy="0.00008922036" iyz="0.00000002694" izz="0.00008724843"/>
+        </inertial>
+    </link>
+    <!-- XACRO -->
+    <xacro:macro name="arm_links_joints" params="side reflect">
+        <joint name="${side}ShoulderPitch" type="revolute">
+            <parent link="torso"/>
+            <child link="${side}Shoulder"/>
+            <limit effort="${150.27*14.3/1000.0}" lower="-2.0857" upper="2.0857" velocity="3.0"/>
+            <origin rpy="0 0 0" xyz="0 ${reflect*0.098} 0.1"/>
+            <axis xyz="0 1 0"/>
+        </joint>
+        <link name="${side}Shoulder">
+            <inertial>
+                <!-- CENTER OF MASS -->
+                <origin rpy="0 0 0" xyz="-0.00165 ${reflect*(-0.02663)} 0.00014"/>
+                <mass value="0.07504"/>
+                <inertia ixx="0.00008428430" ixy="${reflect*-0.00000202802}" ixz="0.00000002338" iyy="0.00001415561" iyz="${reflect*-0.00000001972}" izz="0.00008641949"/>
+            </inertial>
+        </link>
+        <joint name="${side}ElbowYaw" type="revolute">
+            <parent link="${side}ShoulderDummy"/>
+            <child link="${side}Elbow"/>
+            <limit effort="${150.27*14.3/1000.0}" lower="-2.0857" upper="2.0857" velocity="3.0"/>
+            <origin rpy="0 0 0" xyz="0.105 ${reflect*0.015} 0"/>
+            <axis xyz="1 0 0"/>
+        </joint>
+        <link name="${side}Elbow">
+            <inertial>
+                <!-- CENTER OF MASS -->
+                <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
+                <mass value="0.06483"/>
+                <inertia ixx="0.00000559715" ixy="0.00000000421" ixz="0.00000004319" iyy="0.00007543312" iyz="-0.00000000184" izz="0.00007644339"/>
+            </inertial>
+        </link>
+    </xacro:macro>
+    <xacro:arm_links_joints reflect="1" side="L"/>
+    <xacro:arm_links_joints reflect="-1" side="R"/>
+    <!-- JOINTS THAT ARE NOT IDENTICAL TO 2 ENDS -->
+    <joint name="LShoulderRoll" type="revolute">
+        <parent link="LShoulder"/>
+        <child link="LShoulderDummy"/>
+        <limit effort="${173.22*14.3/1000.0}" lower="-0.3142" upper="1.3265" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 0 1"/>
+    </joint>
+    <joint name="RShoulderRoll" type="revolute">
+        <parent link="RShoulder"/>
+        <child link="RShoulderDummy"/>
+        <limit effort="${173.22*14.3/1000.0}" lower="-1.3265" upper="0.3142" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 0 1"/>
+    </joint>
+    <joint name="LElbowRoll" type="revolute">
+        <parent link="LElbow"/>
+        <child link="LElbowDummy"/>
+        <limit effort="${173.22*14.3/1000.0}" lower="-1.5446" upper="-0.0349" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 0 1"/>
+    </joint>
+    <joint name="RElbowRoll" type="revolute">
+        <parent link="RElbow"/>
+        <child link="RElbowDummy"/>
+        <limit effort="${173.22*14.3/1000.0}" lower="0.0349" upper="1.5446" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 0 1"/>
+    </joint>
+    <!-- END OF ARM LINKS/JOINTS -->
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/cameras.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/cameras.xacro
new file mode 100644
index 0000000..8508689
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/cameras.xacro
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- CAMERAS -->
+    <joint name="TopCamera" type="fixed">
+    	<parent link="gaze"/>
+    	<child link="CameraTop"/>
+    	<origin xyz="0.05871 0 0.06364" rpy="0 0.0209 0"/>
+    </joint>
+    <link name="CameraTop"/>
+    <joint name="BottomCamera" type="fixed">
+    	<parent link="gaze"/>
+    	<child link="CameraBottom"/>
+    	<origin xyz="0.05071 0 0.01774" rpy="0 0.6929 0"/>
+    </joint>
+    <link name="CameraBottom"/>
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro
new file mode 100644
index 0000000..2de1db3
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/hands.xacro
@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<!-- H25 Hands -->
+	<xacro:if value="${H25}">
+	    <joint name="LWristYaw" type="revolute">
+	        <parent link="LElbowDummy"/>
+	        <child link="l_wrist"/>
+	        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
+	        <limit effort="${50.61*9.4/1000.0}" lower="-1.8238" upper="1.8238" velocity="3.0"/>
+	    </joint>
+	    <link name="l_wrist">
+	        <xacro:visuals_collisions_wrist_yaw reflect="1" side="L"/>
+	        <inertial>
+	            <!-- CENTER OF MASS -->
+	            <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
+	            <mass value="0.18533"/>
+	            <inertia ixx="0.00007054933" ixy="0.00000571599" ixz="-0.00002247437" iyy="0.00035606232" iyz="0.00000317771" izz="0.00035191933"/>
+	        </inertial>
+	    </link>
+	    <!-- DUMMY REVOLUTE JOINT - JUST THERE TO RETURN VALUES BETWEEN 0.0 AND 1.0 THAT THE ROBOT RETURNS -->
+	    <joint name="LHand" type="revolute">
+	        <parent link="l_wrist"/>
+	        <child link="l_gripper"/>
+	        <origin rpy="0 0 0" xyz="0.05775 0 0"/>
+	        <limit effort="${36.24*9.4/1000.0}" lower="0.0" upper="1.0" velocity="3.0"/>
+	    </joint>
+	    <link name="l_gripper">
+	        <inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	    <joint name="RWristYaw" type="revolute">
+	        <parent link="RElbowDummy"/>
+	        <child link="r_wrist"/>
+	        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
+	        <limit effort="${50.61*9.4/1000.0}" lower="-1.8238" upper="1.8238" velocity="3.0"/>
+	    </joint>
+	    <link name="r_wrist">
+	        <xacro:visuals_collisions_wrist_yaw reflect="-1" side="R"/>
+	        <inertial>
+	            <!-- CENTER OF MASS -->
+	            <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
+	            <mass value="0.18533"/>
+	            <inertia ixx="0.00007054933" ixy="0.00000571599" ixz="-0.00002247437" iyy="0.00035606232" iyz="0.00000317771" izz="0.00035191933"/>
+	        </inertial>
+	    </link>
+	    <!-- DUMMY REVOLUTE JOINT - JUST THERE TO RETURN VALUES BETWEEN 0.0 AND 1.0 THAT THE ROBOT RETURNS -->
+	    <joint name="RHand" type="revolute">
+	        <parent link="r_wrist"/>
+	        <child link="r_gripper"/>
+	        <origin rpy="0 0 0" xyz="0.05775 0 0"/>
+	        <limit effort="${36.24*9.4/1000.0}" lower="0.0" upper="1.0" velocity="3.0"/>
+	    </joint>
+	    <link name="r_gripper">
+	        <inertial> <!-- dummy values for gazebo -->
+	            <mass value="0.2e-5" />
+	            <origin xyz="0 0 0"/>
+	            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
+	        </inertial>
+	    </link>
+	</xacro:if>
+
+	<!-- H21 Hands -->
+	<xacro:unless value="${H25}">
+	    <joint name="LWristYaw" type="fixed">
+	        <parent link="LElbowDummy"/>
+	        <child link="l_wrist"/>
+	        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
+	    </joint>
+	    <link name="l_wrist">
+	        <xacro:visuals_collisions_wrist_yaw reflect="1" side="L"/>
+	        <inertial>
+	            <!-- CENTER OF MASS -->
+	            <origin rpy="0 0 0" xyz="0.06530 0.00114 0.00051"/>
+	            <mass value="0.18415"/>
+	            <inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
+	        </inertial>
+	    </link>
+	    <joint name="RWristYaw" type="fixed">
+	        <parent link="RElbowDummy"/>
+	        <child link="r_wrist"/>
+	        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
+	    </joint>
+	    <link name="r_wrist">
+	        <xacro:visuals_collisions_wrist_yaw reflect="-1" side="R"/>
+	        <inertial>
+	            <!-- CENTER OF MASS -->
+	            <origin rpy="0 0 0" xyz="0.06530 0.00114 0.00051"/>
+	            <mass value="0.18415"/>
+	            <inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
+	        </inertial>
+	    </link>
+	</xacro:unless>
+</robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/head.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/head.xacro
new file mode 100644
index 0000000..91dea24
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/head.xacro
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- TORSO-HEAD JOINT -->
+    <joint name="HeadYaw" type="revolute">
+        <parent link="torso"/>
+        <child link="neck"/>
+        <limit effort="${150.27*14.3/1000.0}" lower="-2.0857" upper="2.0857" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0.1265"/>
+        <axis xyz="0 0 1"/>
+    </joint>
+    <!-- HEAD -->
+    <link name="neck">
+        <xacro:visuals_collisions_head_yaw/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.00001 0.00014 -0.02742"/>
+            <mass value="0.06442"/>
+            <inertia ixx="0.00007499295" ixy="0.00000000157" ixz="-0.00000001834" iyy="0.00007599995" iyz="-0.00000005295" izz="0.00000553373"/>
+        </inertial>
+    </link>
+    <joint name="HeadPitch" type="revolute">
+        <parent link="neck"/>
+        <child link="gaze"/>
+        <limit effort="${173.22*14.3/1000.0}" lower="-0.6720" upper="0.5149" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="gaze">
+        <xacro:visuals_collisions_head_pitch/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.00112 0.00003 0.05258"/>
+            <mass value="0.60533"/>
+            <inertia ixx="0.00263129518" ixy="0.00000878814" ixz="0.00004098466" iyy="0.00249112488" iyz="-0.00002995792" izz="0.00098573565"/>
+        </inertial>
+    </link>
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/imu.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/imu.xacro
new file mode 100644
index 0000000..cb74eeb
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/imu.xacro
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- IMU -->
+    <joint name="torso-imu" type="fixed">
+        <parent link="torso"/>
+        <child link="imu"/>
+        <!-- Accelerometer -->
+        <origin xyz="-0.008 0.00606 0.027" rpy="0 0 0"/>
+        <!-- Gyrometer -->
+        <!-- <origin xyz="-0.008 0.006 0.029" rpy="0 0 0"/> -->
+    </joint>
+    <link name="imu"/>
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/legs.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/legs.xacro
new file mode 100644
index 0000000..c8112ee
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/legs.xacro
@@ -0,0 +1,199 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- LEFT LEG LINKS/JOINTS -->
+    <joint name="LHipYawPitch" type="revolute">
+        <parent link="base_link"/>
+        <child link="LHipYaw"/>
+        <limit effort="${201.3*68/1000.0}" lower="-1.145303" upper="0.740810" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
+        <axis xyz="0 0.707106 -0.707106"/>
+    </joint>
+    <link name="LHipYaw">
+        <xacro:visuals_collisions_hip_yaw_pitch reflect="1" side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
+            <mass value="0.06981"/>
+            <inertia ixx="0.00008150233" ixy="-0.00000499449" ixz="0.00001274817" iyy="0.00010132555" iyz="0.00002345474" izz="0.000062626363"/>
+        </inertial>
+    </link>
+    <joint name="LHipRoll" type="revolute">
+        <parent link="LHipYaw"/>
+        <child link="LHip"/>
+        <limit effort="${201.3*68/1000.0}" lower="-0.379472" upper="0.790477" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="1 0 0"/>
+    </joint>
+    <link name="LHip">
+        <xacro:visuals_collisions_hip_roll side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
+            <mass value="0.13053"/>
+            <inertia ixx="0.00002758354" ixy="-0.00000002233" ixz="-0.00000408164" iyy="0.00009827055" iyz="-0.00000000419" izz="0.00008809973"/>
+        </inertial>
+    </link>
+    <joint name="LHipPitch" type="revolute">
+        <parent link="LHip"/>
+        <child link="LHipDummy"/>
+        <limit effort="${130.85*68/1000.0}" lower="-1.535889" upper="0.484090" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="LHipDummy">
+        <xacro:visuals_collisions_hip_pitch reflect="1" side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
+            <mass value="0.38968"/>
+            <inertia ixx="0.00163671962" ixy="0.00000092451" ixz="0.00008530668" iyy="0.00159107278" iyz="0.00003836160" izz="0.00030374342"/>
+        </inertial>
+    </link>
+    <joint name="LKneePitch" type="revolute">
+        <parent link="LHipDummy"/>
+        <child link="LKnee"/>
+        <limit effort="${130.85*68/1000.0}" lower="-0.092346" upper="2.112528" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="LKnee">
+        <xacro:visuals_collisions_knee_pitch reflect="1" side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
+            <mass value="0.29142"/>
+            <inertia ixx="0.00118207967" ixy="0.00000063362" ixz="0.00003649697" iyy="0.00112865226" iyz="0.00003949523" izz="0.00019322744"/>
+        </inertial>
+    </link>
+    <joint name="LAnklePitch" type="revolute">
+        <parent link="LKnee"/>
+        <child link="l_ankle"/>
+        <limit effort="${130.85*68/1000.0}" lower="-1.189516" upper="0.922747" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="l_ankle">
+        <xacro:visuals_collisions_ankle_pitch side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
+            <mass value="0.13416"/>
+            <inertia ixx="0.00003850978" ixy="-0.00000002634" ixz="0.00000386194" iyy="0.00007426526" iyz="0.00000001834" izz="0.00005486540"/>
+        </inertial>
+    </link>
+    <joint name="LAnkleRoll" type="revolute">
+        <parent link="l_ankle"/>
+        <child link="l_sole"/>
+        <limit effort="${201.3*68/1000.0}" lower="-0.397880" upper="0.769001" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="1 0 0"/>
+    </joint>
+    <link name="l_sole">
+        <xacro:visuals_collisions_ankle_roll side="L"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
+            <mass value="0.16184"/>
+            <inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
+        </inertial>
+    </link>
+    <!-- RIGHT LEG LINKS/JOINTS -->
+    <joint name="RHipYawPitch" type="revolute">
+        <parent link="base_link"/>
+        <child link="RHipYaw"/>
+        <limit effort="${201.3*68/1000.0}" lower="-1.145303" upper="0.740810" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
+        <axis xyz="0 0.707106 0.707106"/>
+        <mimic joint="LHipYawPitch" multiplier="1" offset="0"/>
+    </joint>
+    <link name="RHipYaw">
+        <xacro:visuals_collisions_hip_yaw_pitch reflect="-1" side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.00766 0.01200 0.02716"/>
+            <mass value="0.07118"/>
+            <inertia ixx="0.00008997195" ixy="0.00000500219" ixz="0.00001273525" iyy="0.00010552611" iyz="-0.00002770080" izz="0.00006688724"/>
+        </inertial>
+    </link>
+    <joint name="RHipRoll" type="revolute">
+        <parent link="RHipYaw"/>
+        <child link="RHip"/>
+        <limit effort="${201.3*68/1000.0}" lower="-0.790477" upper="0.379472" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="1 0 0"/>
+    </joint>
+    <link name="RHip">
+        <xacro:visuals_collisions_hip_roll side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00516"/>
+            <mass value="0.13053"/>
+            <inertia ixx="0.00002758654" ixy="-0.00000001919" ixz="-0.00000410822" iyy="0.00009826996" iyz="0.00000000251" izz="0.00008810332"/>
+        </inertial>
+    </link>
+    <joint name="RHipPitch" type="revolute">
+        <parent link="RHip"/>
+        <child link="RHipDummy"/>
+        <limit effort="${130.85*68/1000.0}" lower="-1.535889" upper="0.484090" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="RHipDummy">
+        <xacro:visuals_collisions_hip_pitch reflect="-1" side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.00139 -0.00225 -0.05374"/>
+            <mass value="0.38976"/>
+            <inertia ixx="0.00163748205" ixy="-0.00000083954" ixz="0.00008588301" iyy="0.00159221403" iyz="-0.00003917626" izz="0.00030397824"/>
+        </inertial>
+    </link>
+    <joint name="RKneePitch" type="revolute">
+        <parent link="RHipDummy"/>
+        <child link="RKnee"/>
+        <limit effort="${130.85*68/1000.0}" lower="-0.103083" upper="2.120198" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="RKnee">
+        <xacro:visuals_collisions_knee_pitch reflect="-1" side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.00394 -0.00221 -0.04938"/>
+            <mass value="0.29163"/>
+            <inertia ixx="0.00118282956" ixy="-0.00000089650" ixz="0.00002799690" iyy="0.00112827851" iyz="-0.00003847604" izz="0.00019145277"/>
+        </inertial>
+    </link>
+    <joint name="RAnklePitch" type="revolute">
+        <parent link="RKnee"/>
+        <child link="r_ankle"/>
+        <limit effort="${130.85*68/1000.0}" lower="-1.186448" upper="0.932056" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    <link name="r_ankle">
+        <xacro:visuals_collisions_ankle_pitch side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.00045 -0.00030 0.00684"/>
+            <mass value="0.13415"/>
+            <inertia ixx="0.00003850813" ixy="0.00000006434" ixz="0.00000387466" iyy="0.00007431082" iyz="-0.00000000458" izz="0.00005491312"/>
+        </inertial>
+    </link>
+    <joint name="RAnkleRoll" type="revolute">
+        <parent link="r_ankle"/>
+        <child link="r_sole"/>
+        <limit effort="${201.3*68/1000.0}" lower="-0.768992" upper="0.397935" velocity="3.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <axis xyz="1 0 0"/>
+    </joint>
+    <link name="r_sole">
+        <xacro:visuals_collisions_ankle_roll side="R"/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
+            <mass value="0.16171"/>
+            <inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
+        </inertial>
+    </link>
+    <!-- END OF LEG LINKS/JOINTS -->
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/sonars.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/sonars.xacro
new file mode 100644
index 0000000..1dd9dda
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/sonars.xacro
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">    
+    <!-- SONARS -->
+    <!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
+    <!-- <joint name="RightSonarTransmitter" type="fixed">
+        <parent link="torso"/>
+        <child link="SonarRightTransmitter"/>
+        <origin xyz="0.0537 -0.0341 0.0698"  rpy="0.0 -0.1745 -0.3490"/>
+    </joint>
+    <link name="SonarRightTransmitter"/>
+
+    <joint name="LeftSonarTransmitter" type="fixed">
+        <parent link="torso"/>
+        <child link="SonarLeftTransmitter"/>
+        <origin xyz="0.0537 0.0341 0.0698"  rpy="0.0 -0.1745 0.3490"/>
+    </joint>
+    <link name="SonarLeftTransmitter"/> -->
+    <joint name="RightSonar" type="fixed">
+        <parent link="torso"/>
+        <child link="SonarRight"/>
+        <origin xyz="0.0477 -0.0416 0.0509" rpy="0 0.2618 -0.4363"/>
+    </joint>
+    <link name="SonarRight"/>
+    <joint name="LeftSonar" type="fixed">
+        <parent link="torso"/>
+        <child link="SonarLeft"/>
+        <origin xyz="0.0477 0.0416 0.0509" rpy="0 0.2618 0.4363"/>
+    </joint>
+    <link name="SonarLeft"/>
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/modules/torso.xacro b/nao_dcm_common/nao_dcm_description/urdf/modules/torso.xacro
new file mode 100644
index 0000000..0e9ffbd
--- /dev/null
+++ b/nao_dcm_common/nao_dcm_description/urdf/modules/torso.xacro
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<!-- DUMMY ROOT LINK -->
+    <link name="base_link"/>
+    <joint name="fixed-base" type="fixed">
+        <parent link="base_link"/>
+        <child link="torso"/>
+    </joint>
+    <!-- TORSO -->
+    <link name="torso">
+        <xacro:visuals_collisions_torso/>
+        <inertial>
+            <!-- CENTER OF MASS -->
+            <origin rpy="0 0 0" xyz="-0.00413 0.00009 0.04342"/>
+            <mass value="1.04956"/>
+            <inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.00488013591" iyz="-0.00002707934" izz="0.00161030006"/>
+        </inertial>
+    </link>
+</robot>
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro
index 48c5827..5063b85 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro
@@ -5,10 +5,19 @@
 	<xacro:property name="PI" value="3.14159265359" />
 	<xacro:property name="PI_2" value="1.57079632679" />
 	<xacro:property name="scale" value="0.1" />
+	<xacro:property name="H25" value="false" />
 
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_visuals_collisions.xacro" />
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_gazebo.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H21_structure.xacro" />
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H21_control.xacro" />
 
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/torso.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/head.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/arms.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/hands.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/legs.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/imu.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/sonars.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/cameras.xacro" />
+
 </robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro
deleted file mode 100644
index f314053..0000000
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro
+++ /dev/null
@@ -1,419 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <!-- DUMMY ROOT LINK -->
-    <link name="base_link"/>
-    <joint name="fixed-base" type="fixed">
-        <parent link="base_link"/>
-        <child link="torso"/>
-    </joint>
-    <!-- TORSO -->
-    <link name="torso">
-        <xacro:visuals_collisions_torso/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00413 0.00009 0.04342"/>
-            <mass value="1.04956"/>
-            <inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.0048013591" iyz="-0.00002707934" izz="0.00161030006"/>
-        </inertial>
-    </link>
-    <!-- IMU -->
-    <joint name="torso-imu" type="fixed">
-        <parent link="torso"/>
-        <child link="imu"/>
-        <origin xyz="-0.008 0.00606 0.027" rpy="0 0 0"/>
-    </joint>
-    <link name="imu"/>
-    <!-- SONARS -->
-    <!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
-    <!-- <joint name="RightSonarTransmitter" type="fixed">
-        <parent link="torso"/>
-        <child link="SonarRightTransmitter"/>
-        <origin xyz="0.0537 -0.0341 0.0698"  rpy="0.0 -0.1745 -0.3490"/>
-    </joint>
-    <link name="SonarRightTransmitter"/>
-
-    <joint name="LeftSonarTransmitter" type="fixed">
-        <parent link="torso"/>
-        <child link="SonarLeftTransmitter"/>
-        <origin xyz="0.0537 0.0341 0.0698"  rpy="0.0 -0.1745 0.3490"/>
-    </joint>
-    <link name="SonarLeftTransmitter"/> -->
-    <joint name="RightSonar" type="fixed">
-    	<parent link="torso"/>
-    	<child link="SonarRight"/>
-    	<origin xyz="0.0477 -0.0416 0.0509" rpy="0 0.2618 -0.4363"/>
-    </joint>
-    <link name="SonarRight"/>
-    <joint name="LeftSonar" type="fixed">
-    	<parent link="torso"/>
-    	<child link="SonarLeft"/>
-    	<origin xyz="0.0477 0.0416 0.0509" rpy="0 0.2618 0.4363"/>
-    </joint>
-    <link name="SonarLeft"/>
-    <!-- TORSO-HEAD JOINT -->
-    <joint name="HeadYaw" type="revolute">
-        <parent link="torso"/>
-        <child link="neck"/>
-        <limit effort="5" lower="-2.0857" upper="2.0857" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0.1265"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <!-- HEAD -->
-    <link name="neck">
-        <xacro:visuals_collisions_head_yaw/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00001 0.00014 -0.02742"/>
-            <mass value="0.06442"/>
-            <inertia ixx="0.00007499295" ixy="0.00000000157" ixz="-0.00000001834" iyy="0.00007599995" iyz="-0.00000005295" izz="0.00000553373"/>
-        </inertial>
-    </link>
-    <joint name="HeadPitch" type="revolute">
-        <parent link="neck"/>
-        <child link="gaze"/>
-        <limit effort="5" lower="-0.6720" upper="0.5149" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="gaze">
-        <xacro:visuals_collisions_head_pitch/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00112 0.00003 0.05258"/>
-            <mass value="0.6533"/>
-            <inertia ixx="0.00263129518" ixy="0.00000878814" ixz="0.00004098466" iyy="0.00249112488" iyz="-0.00002995792" izz="0.00098573565"/>
-        </inertial>
-    </link>
-    <!-- CAMERAS -->
-    <joint name="TopCamera" type="fixed">
-    	<parent link="gaze"/>
-    	<child link="CameraTop"/>
-    	<origin xyz="0.05871 0 0.06364" rpy="0 0.0209 0"/>
-    </joint>
-    <link name="CameraTop"/>
-    <joint name="BottomCamera" type="fixed">
-    	<parent link="gaze"/>
-    	<child link="CameraBottom"/>
-    	<origin xyz="0.05071 0 0.01774" rpy="0 0.6929 0"/>
-    </joint>
-    <link name="CameraBottom"/>
-    <!-- ARM LINKS/JOINTS -->
-    <!-- Shoulder Roll Links are different for the 2 arms -->
-    <link name="LShoulderDummy">
-        <xacro:visuals_collisions_shoulder_roll side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
-            <mass value="0.15777"/>
-            <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
-        </inertial>
-    </link>
-    <link name="RShoulderDummy">
-        <xacro:visuals_collisions_shoulder_roll side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
-            <mass value="0.15777"/>
-            <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
-        </inertial>
-    </link>
-    <!-- XACRO -->
-    <xacro:macro name="arm_links_joints" params="side reflect">
-        <joint name="${side}ShoulderPitch" type="revolute">
-            <parent link="torso"/>
-            <child link="${side}Shoulder"/>
-            <limit effort="5" lower="-2.0857" upper="2.0857" velocity="3.0"/>
-            <origin rpy="0 0 0" xyz="0 ${reflect*0.098} 0.1"/>
-            <axis xyz="0 1 0"/>
-        </joint>
-        <link name="${side}Shoulder">
-            <inertial>
-                <!-- CENTER OF MASS -->
-                <origin rpy="0 0 0" xyz="-0.00165 ${reflect*(-0.02663)} 0.00014"/>
-                <mass value="0.07504"/>
-                <inertia ixx="0.00008428430" ixy="${reflect*-0.00000202802}" ixz="0.00000002338" iyy="0.00001415561" iyz="${reflect*-0.00000001972}" izz="0.00008641949"/>
-            </inertial>
-        </link>
-        <joint name="${side}ElbowYaw" type="revolute">
-            <parent link="${side}ShoulderDummy"/>
-            <child link="${side}Elbow"/>
-            <limit effort="5" lower="-2.0857" upper="2.0857" velocity="3.0"/>
-            <origin rpy="0 0 0" xyz="0.105 ${reflect*0.015} 0"/>
-            <axis xyz="1 0 0"/>
-        </joint>
-        <link name="${side}Elbow">
-            <inertial>
-                <!-- CENTER OF MASS -->
-                <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
-                <mass value="0.06483"/>
-                <inertia ixx="0.00000559715" ixy="0.00000000421" ixz="0.00000004319" iyy="0.00007543312" iyz="-0.00000000184" izz="0.00007644339"/>
-            </inertial>
-        </link>
-        <link name="${side}ElbowDummy">
-            <xacro:visuals_collisions_elbow_roll side="${side}"/>
-            <inertial>
-                <!-- CENTER OF MASS -->
-                <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
-                <mass value="0.07761"/>
-                <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
-            </inertial>
-        </link>
-    </xacro:macro>
-    <xacro:arm_links_joints reflect="1" side="L"/>
-    <xacro:arm_links_joints reflect="-1" side="R"/>
-    <!-- JOINTS/LINKS THAT NEED TO BE NAMED FOR REP-120 -->
-    <joint name="LWristYaw" type="fixed">
-        <parent link="LElbowDummy"/>
-        <child link="l_wrist"/>
-        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
-    </joint>
-    <link name="l_wrist">
-        <xacro:visuals_collisions_wrist_yaw reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.06530 0.00114 0.00051"/>
-            <mass value="0.18415"/>
-            <inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
-        </inertial>
-    </link>
-    <joint name="RWristYaw" type="fixed">
-        <parent link="RElbowDummy"/>
-        <child link="r_wrist"/>
-        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
-    </joint>
-    <link name="r_wrist">
-        <xacro:visuals_collisions_wrist_yaw reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.06530 0.00114 0.00051"/>
-            <mass value="0.18415"/>
-            <inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
-        </inertial>
-    </link>
-    <!-- JOINTS THAT ARE NOT IDENTICAL TO 2 ENDS -->
-    <joint name="LShoulderRoll" type="revolute">
-        <parent link="LShoulder"/>
-        <child link="LShoulderDummy"/>
-        <limit effort="5" lower="-0.3142" upper="1.3265" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <joint name="RShoulderRoll" type="revolute">
-        <parent link="RShoulder"/>
-        <child link="RShoulderDummy"/>
-        <limit effort="5" lower="-1.3265" upper="0.3142" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <joint name="LElbowRoll" type="revolute">
-        <parent link="LElbow"/>
-        <child link="LElbowDummy"/>
-        <limit effort="5" lower="-1.5446" upper="-0.0349" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <joint name="RElbowRoll" type="revolute">
-        <parent link="RElbow"/>
-        <child link="RElbowDummy"/>
-        <limit effort="5" lower="0.0349" upper="1.5446" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <!-- END OF ARM LINKS/JOINTS -->
-    <!-- LEFT LEG LINKS/JOINTS -->
-    <joint name="LHipYawPitch" type="revolute">
-        <parent link="base_link"/>
-        <child link="LHipYaw"/>
-        <limit effort="5" lower="-1.145303" upper="0.740810" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
-        <axis xyz="0 0.707106 -0.707106"/>
-    </joint>
-    <link name="LHipYaw">
-        <xacro:visuals_collisions_hip_yaw_pitch reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
-            <mass value="0.06981"/>
-            <inertia ixx="0.00008150233" ixy="-0.00000499449" ixz="0.00001274817" iyy="0.00010132555" iyz="0.00002345474" izz="0.000062626363"/>
-        </inertial>
-    </link>
-    <joint name="LHipRoll" type="revolute">
-        <parent link="LHipYaw"/>
-        <child link="LHip"/>
-        <limit effort="5" lower="-0.379472" upper="0.790477" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="LHip">
-        <xacro:visuals_collisions_hip_roll side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
-            <mass value="0.13053"/>
-            <inertia ixx="0.00002758354" ixy="-0.00000002233" ixz="-0.00000408164" iyy="0.00009827055" iyz="-0.00000000419" izz="0.00008809973"/>
-        </inertial>
-    </link>
-    <joint name="LHipPitch" type="revolute">
-        <parent link="LHip"/>
-        <child link="LHipDummy"/>
-        <limit effort="5" lower="-1.535889" upper="0.484090" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="LHipDummy">
-        <xacro:visuals_collisions_hip_pitch reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
-            <mass value="0.38968"/>
-            <inertia ixx="0.00163671962" ixy="0.00000092451" ixz="0.00008530668" iyy="0.00159107278" iyz="0.00003836160" izz="0.00030374342"/>
-        </inertial>
-    </link>
-    <joint name="LKneePitch" type="revolute">
-        <parent link="LHipDummy"/>
-        <child link="LKnee"/>
-        <limit effort="5" lower="-0.092346" upper="2.112528" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="LKnee">
-        <xacro:visuals_collisions_knee_pitch reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
-            <mass value="0.29142"/>
-            <inertia ixx="0.00118207967" ixy="0.00000063362" ixz="0.00003649697" iyy="0.00112865226" iyz="0.00003949523" izz="0.00019322744"/>
-        </inertial>
-    </link>
-    <joint name="LAnklePitch" type="revolute">
-        <parent link="LKnee"/>
-        <child link="l_ankle"/>
-        <limit effort="5" lower="-1.189516" upper="0.922747" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="l_ankle">
-        <xacro:visuals_collisions_ankle_pitch side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
-            <mass value="0.13416"/>
-            <inertia ixx="0.00003850978" ixy="-0.00000002634" ixz="0.00000386194" iyy="0.00007426526" iyz="0.00000001834" izz="0.00005486540"/>
-        </inertial>
-    </link>
-    <joint name="LAnkleRoll" type="revolute">
-        <parent link="l_ankle"/>
-        <child link="l_sole"/>
-        <limit effort="5" lower="-0.397880" upper="0.769001" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="l_sole">
-        <xacro:visuals_collisions_ankle_roll side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
-            <mass value="0.16184"/>
-            <inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
-        </inertial>
-    </link>
-    <!-- RIGHT LEG LINKS/JOINTS -->
-    <joint name="RHipYawPitch" type="revolute">
-        <parent link="base_link"/>
-        <child link="RHipYaw"/>
-        <limit effort="5" lower="-1.145303" upper="0.740810" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
-        <axis xyz="0 0.707106 0.707106"/>
-        <mimic joint="LHipYawPitch" multiplier="1" offset="0"/>
-    </joint>
-    <link name="RHipYaw">
-        <xacro:visuals_collisions_hip_yaw_pitch reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00766 0.01200 0.02716"/>
-            <mass value="0.07118"/>
-            <inertia ixx="0.00008997195" ixy="0.00000500219" ixz="0.00001273525" iyy="0.00010552611" iyz="-0.00002770080" izz="0.00006688724"/>
-        </inertial>
-    </link>
-    <joint name="RHipRoll" type="revolute">
-        <parent link="RHipYaw"/>
-        <child link="RHip"/>
-        <limit effort="5" lower="-0.790477" upper="0.379472" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="RHip">
-        <xacro:visuals_collisions_hip_roll side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00516"/>
-            <mass value="0.13053"/>
-            <inertia ixx="0.00002758654" ixy="-0.00000001919" ixz="-0.00000410822" iyy="0.00009826996" iyz="0.00000000251" izz="0.00008810332"/>
-        </inertial>
-    </link>
-    <joint name="RHipPitch" type="revolute">
-        <parent link="RHip"/>
-        <child link="RHipDummy"/>
-        <limit effort="5" lower="-1.535889" upper="0.484090" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="RHipDummy">
-        <xacro:visuals_collisions_hip_pitch reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00139 -0.00225 -0.05374"/>
-            <mass value="0.38976"/>
-            <inertia ixx="0.00163748205" ixy="-0.00000083954" ixz="0.00008588301" iyy="0.00159221403" iyz="-0.00003917626" izz="0.00030397824"/>
-        </inertial>
-    </link>
-    <joint name="RKneePitch" type="revolute">
-        <parent link="RHipDummy"/>
-        <child link="RKnee"/>
-        <limit effort="5" lower="-0.103083" upper="2.120198" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="RKnee">
-        <xacro:visuals_collisions_knee_pitch reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00394 -0.00221 -0.04938"/>
-            <mass value="0.29163"/>
-            <inertia ixx="0.00118282956" ixy="-0.00000089650" ixz="0.00002799690" iyy="0.00112827851" iyz="-0.00003847604" izz="0.00019145277"/>
-        </inertial>
-    </link>
-    <joint name="RAnklePitch" type="revolute">
-        <parent link="RKnee"/>
-        <child link="r_ankle"/>
-        <limit effort="5" lower="-1.186448" upper="0.932056" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="r_ankle">
-        <xacro:visuals_collisions_ankle_pitch side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00045 -0.00030 0.00684"/>
-            <mass value="0.13415"/>
-            <inertia ixx="0.00003850813" ixy="0.00000006434" ixz="0.00000387466" iyy="0.00007431082" iyz="-0.00000000458" izz="0.00005491312"/>
-        </inertial>
-    </link>
-    <joint name="RAnkleRoll" type="revolute">
-        <parent link="r_ankle"/>
-        <child link="r_sole"/>
-        <limit effort="5" lower="-0.768992" upper="0.397935" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="r_sole">
-        <xacro:visuals_collisions_ankle_roll side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
-            <mass value="0.16171"/>
-            <inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
-        </inertial>
-    </link>
-    <!-- END OF LEG LINKS/JOINTS -->
-</robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro
index ed6352b..25a5b72 100644
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro
+++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro
@@ -5,10 +5,19 @@
 	<xacro:property name="PI" value="3.14159265359" />
 	<xacro:property name="PI_2" value="1.57079632679" />
 	<xacro:property name="scale" value="0.1" />
+	<xacro:property name="H25" value="true" />
 
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_visuals_collisions.xacro" />
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_gazebo.xacro" />
-	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H25_structure.xacro" />
 	<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H25_control.xacro" />
 
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/torso.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/head.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/arms.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/hands.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/legs.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/imu.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/sonars.xacro" />
+	<xacro:include filename="$(find nao_dcm_description)/urdf/modules/cameras.xacro" />
+
 </robot>
\ No newline at end of file
diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro
deleted file mode 100644
index 03cc6b9..0000000
--- a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro
+++ /dev/null
@@ -1,449 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <!-- DUMMY ROOT LINK -->
-    <link name="base_link"/>
-    <joint name="fixed-base" type="fixed">
-        <parent link="base_link"/>
-        <child link="torso"/>
-    </joint>
-    <!-- TORSO -->
-    <link name="torso">
-        <xacro:visuals_collisions_torso/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00413 0.00009 0.04342"/>
-            <mass value="1.04956"/>
-            <inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.0048013591" iyz="-0.00002707934" izz="0.00161030006"/>
-        </inertial>
-    </link>
-    <!-- IMU -->
-    <joint name="torso-imu" type="fixed">
-        <parent link="torso"/>
-        <child link="imu"/>
-        <origin xyz="-0.008 0.00606 0.027" rpy="0 0 0"/>
-    </joint>
-    <link name="imu"/>
-    <!-- SONARS -->
-    <!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
-    <!-- <joint name="RightSonarTransmitter" type="fixed">
-        <parent link="torso"/>
-        <child link="SonarRightTransmitter"/>
-        <origin xyz="0.0537 -0.0341 0.0698"  rpy="0.0 -0.1745 -0.3490"/>
-    </joint>
-    <link name="SonarRightTransmitter"/>
-
-    <joint name="LeftSonarTransmitter" type="fixed">
-        <parent link="torso"/>
-        <child link="SonarLeftTransmitter"/>
-        <origin xyz="0.0537 0.0341 0.0698"  rpy="0.0 -0.1745 0.3490"/>
-    </joint>
-    <link name="SonarLeftTransmitter"/> -->
-    <joint name="RightSonar" type="fixed">
-    	<parent link="torso"/>
-    	<child link="SonarRight"/>
-    	<origin xyz="0.0477 -0.0416 0.0509" rpy="0 0.2618 -0.4363"/>
-    </joint>
-    <link name="SonarRight"/>
-    <joint name="LeftSonar" type="fixed">
-    	<parent link="torso"/>
-    	<child link="SonarLeft"/>
-    	<origin xyz="0.0477 0.0416 0.0509" rpy="0 0.2618 0.4363"/>
-    </joint>
-    <link name="SonarLeft"/>
-    <!-- TORSO-HEAD JOINT -->
-    <joint name="HeadYaw" type="revolute">
-        <parent link="torso"/>
-        <child link="neck"/>
-        <limit effort="5" lower="-2.0857" upper="2.0857" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0.1265"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <!-- HEAD -->
-    <link name="neck">
-        <xacro:visuals_collisions_head_yaw/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00001 0.00014 -0.02742"/>
-            <mass value="0.06442"/>
-            <inertia ixx="0.00007499295" ixy="0.00000000157" ixz="-0.00000001834" iyy="0.00007599995" iyz="-0.00000005295" izz="0.00000553373"/>
-        </inertial>
-    </link>
-    <joint name="HeadPitch" type="revolute">
-        <parent link="neck"/>
-        <child link="gaze"/>
-        <limit effort="5" lower="-0.6720" upper="0.5149" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="gaze">
-        <xacro:visuals_collisions_head_pitch/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00112 0.00003 0.05258"/>
-            <mass value="0.6533"/>
-            <inertia ixx="0.00263129518" ixy="0.00000878814" ixz="0.00004098466" iyy="0.00249112488" iyz="-0.00002995792" izz="0.00098573565"/>
-        </inertial>
-    </link>
-    <!-- CAMERAS -->
-    <joint name="TopCamera" type="fixed">
-    	<parent link="gaze"/>
-    	<child link="CameraTop"/>
-    	<origin xyz="0.05871 0 0.06364" rpy="0 0.0209 0"/>
-    </joint>
-    <link name="CameraTop"/>
-    <joint name="BottomCamera" type="fixed">
-    	<parent link="gaze"/>
-    	<child link="CameraBottom"/>
-    	<origin xyz="0.05071 0 0.01774" rpy="0 0.6929 0"/>
-    </joint>
-    <link name="CameraBottom"/>
-    <!-- ARM LINKS/JOINTS -->
-    <!-- Shoulder Roll Links are different for the 2 arms -->
-    <link name="LShoulderDummy">
-        <xacro:visuals_collisions_shoulder_roll side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
-            <mass value="0.15777"/>
-            <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
-        </inertial>
-    </link>
-    <link name="RShoulderDummy">
-        <xacro:visuals_collisions_shoulder_roll side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
-            <mass value="0.15777"/>
-            <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
-        </inertial>
-    </link>
-    <!-- XACRO -->
-    <xacro:macro name="arm_links_joints" params="side reflect">
-        <joint name="${side}ShoulderPitch" type="revolute">
-            <parent link="torso"/>
-            <child link="${side}Shoulder"/>
-            <limit effort="5" lower="-2.0857" upper="2.0857" velocity="3.0"/>
-            <origin rpy="0 0 0" xyz="0 ${reflect*0.098} 0.1"/>
-            <axis xyz="0 1 0"/>
-        </joint>
-        <link name="${side}Shoulder">
-            <inertial>
-                <!-- CENTER OF MASS -->
-                <origin rpy="0 0 0" xyz="-0.00165 ${reflect*(-0.02663)} 0.00014"/>
-                <mass value="0.07504"/>
-                <inertia ixx="0.00008428430" ixy="${reflect*-0.00000202802}" ixz="0.00000002338" iyy="0.00001415561" iyz="${reflect*-0.00000001972}" izz="0.00008641949"/>
-            </inertial>
-        </link>
-        <joint name="${side}ElbowYaw" type="revolute">
-            <parent link="${side}ShoulderDummy"/>
-            <child link="${side}Elbow"/>
-            <limit effort="5" lower="-2.0857" upper="2.0857" velocity="3.0"/>
-            <origin rpy="0 0 0" xyz="0.105 ${reflect*0.015} 0"/>
-            <axis xyz="1 0 0"/>
-        </joint>
-        <link name="${side}Elbow">
-            <inertial>
-                <!-- CENTER OF MASS -->
-                <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
-                <mass value="0.06483"/>
-                <inertia ixx="0.00000559715" ixy="0.00000000421" ixz="0.00000004319" iyy="0.00007543312" iyz="-0.00000000184" izz="0.00007644339"/>
-            </inertial>
-        </link>
-        <link name="${side}ElbowDummy">
-            <xacro:visuals_collisions_elbow_roll side="${side}"/>
-            <inertial>
-                <!-- CENTER OF MASS -->
-                <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
-                <mass value="0.07761"/>
-                <inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
-            </inertial>
-        </link>
-    </xacro:macro>
-    <xacro:arm_links_joints reflect="1" side="L"/>
-    <xacro:arm_links_joints reflect="-1" side="R"/>
-    <!-- JOINTS/LINKS THAT NEED TO BE NAMED FOR REP-120 -->
-    <joint name="LWristYaw" type="revolute">
-        <parent link="LElbowDummy"/>
-        <child link="l_wrist"/>
-        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
-        <limit effort="0.49" lower="-1.8238" upper="1.8238" velocity="3.0"/>
-    </joint>
-    <link name="l_wrist">
-        <xacro:visuals_collisions_wrist_yaw reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
-            <mass value="0.18533"/>
-            <inertia ixx="0.00007054933" ixy="0.00000571599" ixz="-0.00002247437" iyy="0.00035606232" iyz="0.00000317771" izz="0.00035191933"/>
-        </inertial>
-    </link>
-    <!-- DUMMY REVOLUTE JOINT - JUST THERE TO RETURN VALUES BETWEEN 0.0 AND 1.0 THAT THE ROBOT RETURNS -->
-    <joint name="LHand" type="revolute">
-        <parent link="l_wrist"/>
-        <child link="l_gripper"/>
-        <origin rpy="0 0 0" xyz="0.05775 0 0"/>
-        <limit effort="0.49" lower="0.0" upper="1.0" velocity="3.0"/>
-    </joint>
-    <link name="l_gripper">
-        <inertial> <!-- dummy values for gazebo -->
-            <mass value="1e-5" />
-            <origin xyz="0 0 0"/>
-            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
-        </inertial>
-    </link>
-    <joint name="RWristYaw" type="revolute">
-        <parent link="RElbowDummy"/>
-        <child link="r_wrist"/>
-        <origin rpy="0 0 0" xyz="0.05595 0 0"/>
-        <limit effort="0.49" lower="-1.8238" upper="1.8238" velocity="3.0"/>
-    </joint>
-    <link name="r_wrist">
-        <xacro:visuals_collisions_wrist_yaw reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
-            <mass value="0.18533"/>
-            <inertia ixx="0.00007054933" ixy="0.00000571599" ixz="-0.00002247437" iyy="0.00035606232" iyz="0.00000317771" izz="0.00035191933"/>
-        </inertial>
-    </link>
-    <!-- DUMMY REVOLUTE JOINT - JUST THERE TO RETURN VALUES BETWEEN 0.0 AND 1.0 THAT THE ROBOT RETURNS -->
-    <joint name="RHand" type="revolute">
-        <parent link="r_wrist"/>
-        <child link="r_gripper"/>
-        <origin rpy="0 0 0" xyz="0.05775 0 0"/>
-        <limit effort="0.49" lower="0.0" upper="1.0" velocity="3.0"/>
-    </joint>
-    <link name="r_gripper">
-        <inertial> <!-- dummy values for gazebo -->
-            <mass value="1e-5" />
-            <origin xyz="0 0 0"/>
-            <inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
-        </inertial>
-    </link>
-    <!-- JOINTS THAT ARE NOT IDENTICAL TO 2 ENDS -->
-    <joint name="LShoulderRoll" type="revolute">
-        <parent link="LShoulder"/>
-        <child link="LShoulderDummy"/>
-        <limit effort="5" lower="-0.3142" upper="1.3265" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <joint name="RShoulderRoll" type="revolute">
-        <parent link="RShoulder"/>
-        <child link="RShoulderDummy"/>
-        <limit effort="5" lower="-1.3265" upper="0.3142" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <joint name="LElbowRoll" type="revolute">
-        <parent link="LElbow"/>
-        <child link="LElbowDummy"/>
-        <limit effort="5" lower="-1.5446" upper="-0.0349" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <joint name="RElbowRoll" type="revolute">
-        <parent link="RElbow"/>
-        <child link="RElbowDummy"/>
-        <limit effort="5" lower="0.0349" upper="1.5446" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 0 1"/>
-    </joint>
-    <!-- END OF ARM LINKS/JOINTS -->
-    <!-- LEFT LEG LINKS/JOINTS -->
-    <joint name="LHipYawPitch" type="revolute">
-        <parent link="base_link"/>
-        <child link="LHipYaw"/>
-        <limit effort="1.61" lower="-1.145303" upper="0.740810" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
-        <axis xyz="0 0.707106 -0.707106"/>
-    </joint>
-    <link name="LHipYaw">
-        <xacro:visuals_collisions_hip_yaw_pitch reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
-            <mass value="0.06981"/>
-            <inertia ixx="0.00008150233" ixy="-0.00000499449" ixz="0.00001274817" iyy="0.00010132555" iyz="0.00002345474" izz="0.000062626363"/>
-        </inertial>
-    </link>
-    <joint name="LHipRoll" type="revolute">
-        <parent link="LHipYaw"/>
-        <child link="LHip"/>
-        <limit effort="1.61" lower="-0.379472" upper="0.790477" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="LHip">
-        <xacro:visuals_collisions_hip_roll side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
-            <mass value="0.13053"/>
-            <inertia ixx="0.00002758354" ixy="-0.00000002233" ixz="-0.00000408164" iyy="0.00009827055" iyz="-0.00000000419" izz="0.00008809973"/>
-        </inertial>
-    </link>
-    <joint name="LHipPitch" type="revolute">
-        <parent link="LHip"/>
-        <child link="LHipDummy"/>
-        <limit effort="1.61" lower="-1.535889" upper="0.484090" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="LHipDummy">
-        <xacro:visuals_collisions_hip_pitch reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
-            <mass value="0.38968"/>
-            <inertia ixx="0.00163671962" ixy="0.00000092451" ixz="0.00008530668" iyy="0.00159107278" iyz="0.00003836160" izz="0.00030374342"/>
-        </inertial>
-    </link>
-    <joint name="LKneePitch" type="revolute">
-        <parent link="LHipDummy"/>
-        <child link="LKnee"/>
-        <limit effort="1.61" lower="-0.092346" upper="2.112528" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="LKnee">
-        <xacro:visuals_collisions_knee_pitch reflect="1" side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
-            <mass value="0.29142"/>
-            <inertia ixx="0.00118207967" ixy="0.00000063362" ixz="0.00003649697" iyy="0.00112865226" iyz="0.00003949523" izz="0.00019322744"/>
-        </inertial>
-    </link>
-    <joint name="LAnklePitch" type="revolute">
-        <parent link="LKnee"/>
-        <child link="l_ankle"/>
-        <limit effort="1.61" lower="-1.189516" upper="0.922747" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="l_ankle">
-        <xacro:visuals_collisions_ankle_pitch side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
-            <mass value="0.13416"/>
-            <inertia ixx="0.00003850978" ixy="-0.00000002634" ixz="0.00000386194" iyy="0.00007426526" iyz="0.00000001834" izz="0.00005486540"/>
-        </inertial>
-    </link>
-    <joint name="LAnkleRoll" type="revolute">
-        <parent link="l_ankle"/>
-        <child link="l_sole"/>
-        <limit effort="1.61" lower="-0.397880" upper="0.769001" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="l_sole">
-        <xacro:visuals_collisions_ankle_roll side="L"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
-            <mass value="0.16184"/>
-            <inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
-        </inertial>
-    </link>
-    <!-- RIGHT LEG LINKS/JOINTS -->
-    <joint name="RHipYawPitch" type="revolute">
-        <parent link="base_link"/>
-        <child link="RHipYaw"/>
-        <limit effort="1.61" lower="-1.145303" upper="0.740810" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
-        <axis xyz="0 0.707106 0.707106"/>
-        <mimic joint="LHipYawPitch" multiplier="1" offset="0"/>
-    </joint>
-    <link name="RHipYaw">
-        <xacro:visuals_collisions_hip_yaw_pitch reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.00766 0.01200 0.02716"/>
-            <mass value="0.07118"/>
-            <inertia ixx="0.00008997195" ixy="0.00000500219" ixz="0.00001273525" iyy="0.00010552611" iyz="-0.00002770080" izz="0.00006688724"/>
-        </inertial>
-    </link>
-    <joint name="RHipRoll" type="revolute">
-        <parent link="RHipYaw"/>
-        <child link="RHip"/>
-        <limit effort="1.61" lower="-0.790477" upper="0.379472" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="RHip">
-        <xacro:visuals_collisions_hip_roll side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00516"/>
-            <mass value="0.13053"/>
-            <inertia ixx="0.00002758654" ixy="-0.00000001919" ixz="-0.00000410822" iyy="0.00009826996" iyz="0.00000000251" izz="0.00008810332"/>
-        </inertial>
-    </link>
-    <joint name="RHipPitch" type="revolute">
-        <parent link="RHip"/>
-        <child link="RHipDummy"/>
-        <limit effort="1.61" lower="-1.535889" upper="0.484090" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="RHipDummy">
-        <xacro:visuals_collisions_hip_pitch reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00139 -0.00225 -0.05374"/>
-            <mass value="0.38976"/>
-            <inertia ixx="0.00163748205" ixy="-0.00000083954" ixz="0.00008588301" iyy="0.00159221403" iyz="-0.00003917626" izz="0.00030397824"/>
-        </inertial>
-    </link>
-    <joint name="RKneePitch" type="revolute">
-        <parent link="RHipDummy"/>
-        <child link="RKnee"/>
-        <limit effort="1.61" lower="-0.103083" upper="2.120198" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="RKnee">
-        <xacro:visuals_collisions_knee_pitch reflect="-1" side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00394 -0.00221 -0.04938"/>
-            <mass value="0.29163"/>
-            <inertia ixx="0.00118282956" ixy="-0.00000089650" ixz="0.00002799690" iyy="0.00112827851" iyz="-0.00003847604" izz="0.00019145277"/>
-        </inertial>
-    </link>
-    <joint name="RAnklePitch" type="revolute">
-        <parent link="RKnee"/>
-        <child link="r_ankle"/>
-        <limit effort="1.61" lower="-1.186448" upper="0.932056" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
-        <axis xyz="0 1 0"/>
-    </joint>
-    <link name="r_ankle">
-        <xacro:visuals_collisions_ankle_pitch side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.00045 -0.00030 0.00684"/>
-            <mass value="0.13415"/>
-            <inertia ixx="0.00003850813" ixy="0.00000006434" ixz="0.00000387466" iyy="0.00007431082" iyz="-0.00000000458" izz="0.00005491312"/>
-        </inertial>
-    </link>
-    <joint name="RAnkleRoll" type="revolute">
-        <parent link="r_ankle"/>
-        <child link="r_sole"/>
-        <limit effort="1.61" lower="-0.768992" upper="0.397935" velocity="3.0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <axis xyz="1 0 0"/>
-    </joint>
-    <link name="r_sole">
-        <xacro:visuals_collisions_ankle_roll side="R"/>
-        <inertial>
-            <!-- CENTER OF MASS -->
-            <origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
-            <mass value="0.16171"/>
-            <inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
-        </inertial>
-    </link>
-    <!-- END OF LEG LINKS/JOINTS -->
-</robot>
\ No newline at end of file