diff --git a/resources/robots/g1/meshes/head_link.STL b/resources/robots/g1/meshes/head_link.STL
new file mode 100644
index 0000000..455274f
Binary files /dev/null and b/resources/robots/g1/meshes/head_link.STL differ
diff --git a/resources/robots/g1/meshes/left_ankle_pitch_link.STL b/resources/robots/g1/meshes/left_ankle_pitch_link.STL
new file mode 100644
index 0000000..c291f60
Binary files /dev/null and b/resources/robots/g1/meshes/left_ankle_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_ankle_roll_link.STL b/resources/robots/g1/meshes/left_ankle_roll_link.STL
new file mode 100644
index 0000000..f2b6ae7
Binary files /dev/null and b/resources/robots/g1/meshes/left_ankle_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_elbow_pitch_link.STL b/resources/robots/g1/meshes/left_elbow_pitch_link.STL
new file mode 100644
index 0000000..5ba2fde
Binary files /dev/null and b/resources/robots/g1/meshes/left_elbow_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_elbow_roll_link.STL b/resources/robots/g1/meshes/left_elbow_roll_link.STL
new file mode 100644
index 0000000..9741ac0
Binary files /dev/null and b/resources/robots/g1/meshes/left_elbow_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_five_link.STL b/resources/robots/g1/meshes/left_five_link.STL
new file mode 100644
index 0000000..cc35d51
Binary files /dev/null and b/resources/robots/g1/meshes/left_five_link.STL differ
diff --git a/resources/robots/g1/meshes/left_four_link.STL b/resources/robots/g1/meshes/left_four_link.STL
new file mode 100644
index 0000000..6392be7
Binary files /dev/null and b/resources/robots/g1/meshes/left_four_link.STL differ
diff --git a/resources/robots/g1/meshes/left_hip_pitch_link.STL b/resources/robots/g1/meshes/left_hip_pitch_link.STL
new file mode 100644
index 0000000..1e56afc
Binary files /dev/null and b/resources/robots/g1/meshes/left_hip_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_hip_roll_link.STL b/resources/robots/g1/meshes/left_hip_roll_link.STL
new file mode 100644
index 0000000..d1df2f4
Binary files /dev/null and b/resources/robots/g1/meshes/left_hip_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_hip_yaw_link.STL b/resources/robots/g1/meshes/left_hip_yaw_link.STL
new file mode 100644
index 0000000..deadd3c
Binary files /dev/null and b/resources/robots/g1/meshes/left_hip_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/left_knee_link.STL b/resources/robots/g1/meshes/left_knee_link.STL
new file mode 100644
index 0000000..e0095a4
Binary files /dev/null and b/resources/robots/g1/meshes/left_knee_link.STL differ
diff --git a/resources/robots/g1/meshes/left_one_link.STL b/resources/robots/g1/meshes/left_one_link.STL
new file mode 100644
index 0000000..6ee97bd
Binary files /dev/null and b/resources/robots/g1/meshes/left_one_link.STL differ
diff --git a/resources/robots/g1/meshes/left_palm_link.STL b/resources/robots/g1/meshes/left_palm_link.STL
new file mode 100644
index 0000000..524a0e2
Binary files /dev/null and b/resources/robots/g1/meshes/left_palm_link.STL differ
diff --git a/resources/robots/g1/meshes/left_shoulder_pitch_link.STL b/resources/robots/g1/meshes/left_shoulder_pitch_link.STL
new file mode 100644
index 0000000..e54d656
Binary files /dev/null and b/resources/robots/g1/meshes/left_shoulder_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/left_shoulder_roll_link.STL b/resources/robots/g1/meshes/left_shoulder_roll_link.STL
new file mode 100644
index 0000000..75dc809
Binary files /dev/null and b/resources/robots/g1/meshes/left_shoulder_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/left_shoulder_yaw_link.STL b/resources/robots/g1/meshes/left_shoulder_yaw_link.STL
new file mode 100644
index 0000000..4e5a7c9
Binary files /dev/null and b/resources/robots/g1/meshes/left_shoulder_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/left_six_link.STL b/resources/robots/g1/meshes/left_six_link.STL
new file mode 100644
index 0000000..2739846
Binary files /dev/null and b/resources/robots/g1/meshes/left_six_link.STL differ
diff --git a/resources/robots/g1/meshes/left_three_link.STL b/resources/robots/g1/meshes/left_three_link.STL
new file mode 100644
index 0000000..cc35d51
Binary files /dev/null and b/resources/robots/g1/meshes/left_three_link.STL differ
diff --git a/resources/robots/g1/meshes/left_two_link.STL b/resources/robots/g1/meshes/left_two_link.STL
new file mode 100644
index 0000000..b85b645
Binary files /dev/null and b/resources/robots/g1/meshes/left_two_link.STL differ
diff --git a/resources/robots/g1/meshes/left_zero_link.STL b/resources/robots/g1/meshes/left_zero_link.STL
new file mode 100644
index 0000000..f50147a
Binary files /dev/null and b/resources/robots/g1/meshes/left_zero_link.STL differ
diff --git a/resources/robots/g1/meshes/logo_link.STL b/resources/robots/g1/meshes/logo_link.STL
new file mode 100644
index 0000000..20f7bba
Binary files /dev/null and b/resources/robots/g1/meshes/logo_link.STL differ
diff --git a/resources/robots/g1/meshes/pelvis.STL b/resources/robots/g1/meshes/pelvis.STL
new file mode 100644
index 0000000..5877dee
Binary files /dev/null and b/resources/robots/g1/meshes/pelvis.STL differ
diff --git a/resources/robots/g1/meshes/pelvis_contour_link.STL b/resources/robots/g1/meshes/pelvis_contour_link.STL
new file mode 100644
index 0000000..72fa4ac
Binary files /dev/null and b/resources/robots/g1/meshes/pelvis_contour_link.STL differ
diff --git a/resources/robots/g1/meshes/right_ankle_pitch_link.STL b/resources/robots/g1/meshes/right_ankle_pitch_link.STL
new file mode 100644
index 0000000..36e3a64
Binary files /dev/null and b/resources/robots/g1/meshes/right_ankle_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_ankle_roll_link.STL b/resources/robots/g1/meshes/right_ankle_roll_link.STL
new file mode 100644
index 0000000..d0a2657
Binary files /dev/null and b/resources/robots/g1/meshes/right_ankle_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_elbow_pitch_link.STL b/resources/robots/g1/meshes/right_elbow_pitch_link.STL
new file mode 100644
index 0000000..a62a422
Binary files /dev/null and b/resources/robots/g1/meshes/right_elbow_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_elbow_roll_link.STL b/resources/robots/g1/meshes/right_elbow_roll_link.STL
new file mode 100644
index 0000000..6010e8b
Binary files /dev/null and b/resources/robots/g1/meshes/right_elbow_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_five_link.STL b/resources/robots/g1/meshes/right_five_link.STL
new file mode 100644
index 0000000..37f9dc2
Binary files /dev/null and b/resources/robots/g1/meshes/right_five_link.STL differ
diff --git a/resources/robots/g1/meshes/right_four_link.STL b/resources/robots/g1/meshes/right_four_link.STL
new file mode 100644
index 0000000..be81a79
Binary files /dev/null and b/resources/robots/g1/meshes/right_four_link.STL differ
diff --git a/resources/robots/g1/meshes/right_hip_pitch_link.STL b/resources/robots/g1/meshes/right_hip_pitch_link.STL
new file mode 100644
index 0000000..66b47ea
Binary files /dev/null and b/resources/robots/g1/meshes/right_hip_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_hip_roll_link.STL b/resources/robots/g1/meshes/right_hip_roll_link.STL
new file mode 100644
index 0000000..021dd7f
Binary files /dev/null and b/resources/robots/g1/meshes/right_hip_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_hip_yaw_link.STL b/resources/robots/g1/meshes/right_hip_yaw_link.STL
new file mode 100644
index 0000000..330e55a
Binary files /dev/null and b/resources/robots/g1/meshes/right_hip_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/right_knee_link.STL b/resources/robots/g1/meshes/right_knee_link.STL
new file mode 100644
index 0000000..7be6860
Binary files /dev/null and b/resources/robots/g1/meshes/right_knee_link.STL differ
diff --git a/resources/robots/g1/meshes/right_one_link.STL b/resources/robots/g1/meshes/right_one_link.STL
new file mode 100644
index 0000000..93e103a
Binary files /dev/null and b/resources/robots/g1/meshes/right_one_link.STL differ
diff --git a/resources/robots/g1/meshes/right_palm_link.STL b/resources/robots/g1/meshes/right_palm_link.STL
new file mode 100644
index 0000000..7cd39cd
Binary files /dev/null and b/resources/robots/g1/meshes/right_palm_link.STL differ
diff --git a/resources/robots/g1/meshes/right_shoulder_pitch_link.STL b/resources/robots/g1/meshes/right_shoulder_pitch_link.STL
new file mode 100644
index 0000000..7e1d01d
Binary files /dev/null and b/resources/robots/g1/meshes/right_shoulder_pitch_link.STL differ
diff --git a/resources/robots/g1/meshes/right_shoulder_roll_link.STL b/resources/robots/g1/meshes/right_shoulder_roll_link.STL
new file mode 100644
index 0000000..f653871
Binary files /dev/null and b/resources/robots/g1/meshes/right_shoulder_roll_link.STL differ
diff --git a/resources/robots/g1/meshes/right_shoulder_yaw_link.STL b/resources/robots/g1/meshes/right_shoulder_yaw_link.STL
new file mode 100644
index 0000000..01aa95a
Binary files /dev/null and b/resources/robots/g1/meshes/right_shoulder_yaw_link.STL differ
diff --git a/resources/robots/g1/meshes/right_six_link.STL b/resources/robots/g1/meshes/right_six_link.STL
new file mode 100644
index 0000000..be81a79
Binary files /dev/null and b/resources/robots/g1/meshes/right_six_link.STL differ
diff --git a/resources/robots/g1/meshes/right_three_link.STL b/resources/robots/g1/meshes/right_three_link.STL
new file mode 100644
index 0000000..37f9dc2
Binary files /dev/null and b/resources/robots/g1/meshes/right_three_link.STL differ
diff --git a/resources/robots/g1/meshes/right_two_link.STL b/resources/robots/g1/meshes/right_two_link.STL
new file mode 100644
index 0000000..1af670c
Binary files /dev/null and b/resources/robots/g1/meshes/right_two_link.STL differ
diff --git a/resources/robots/g1/meshes/right_zero_link.STL b/resources/robots/g1/meshes/right_zero_link.STL
new file mode 100644
index 0000000..4fd999b
Binary files /dev/null and b/resources/robots/g1/meshes/right_zero_link.STL differ
diff --git a/resources/robots/g1/meshes/torso_link.STL b/resources/robots/g1/meshes/torso_link.STL
new file mode 100644
index 0000000..b6a6d12
Binary files /dev/null and b/resources/robots/g1/meshes/torso_link.STL differ
diff --git a/resources/robots/g1/urdf/g1.urdf b/resources/robots/g1/urdf/g1.urdf
new file mode 100644
index 0000000..ce3a36a
--- /dev/null
+++ b/resources/robots/g1/urdf/g1.urdf
@@ -0,0 +1,1295 @@
+<robot name="g1">
+
+  <link name="pelvis">
+    <inertial>
+      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
+      <mass value="2.86"/>
+      <inertia ixx="0.0079143" ixy="0" ixz="1.6E-06" iyy="0.0069837" iyz="0" izz="0.0059404"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/pelvis.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/pelvis.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="pelvis_contour_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/pelvis_contour_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="pelvis_contour_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="pelvis_contour_link"/>
+  </joint>
+
+  <!-- Leg -->
+  <link name="left_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="-1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="-4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_pitch_joint" type="revolute">
+    <origin xyz="0 0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="left_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 -0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="-3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="-1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_roll_joint" type="revolute">
+    <origin xyz="0 0.0523 0" rpy="0 0 0"/>
+    <parent link="left_hip_pitch_link"/>
+    <child link="left_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.26" upper="2.53" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 -0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="-0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="-0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 -0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="left_hip_roll_link"/>
+    <child link="left_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="left_knee_link">
+    <inertial>
+      <origin xyz="0.005505 0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="-0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.015" length="0.15"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_knee_joint" type="revolute">
+    <origin xyz="-0.078292 -0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="left_hip_yaw_link"/>
+    <child link="left_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" effort="139" velocity="20"/>
+  </joint>
+  <link name="left_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
+      <mass value="0.074"/>
+      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="left_knee_link"/>
+    <child link="left_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="left_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="-1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="left_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
+    <parent link="left_ankle_pitch_link"/>
+    <child link="left_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.2618" upper="0.2618" effort="40" velocity="53"/>
+  </joint>
+  <link name="right_hip_pitch_link">
+    <inertial>
+      <origin xyz="0.001962 -0.049392 -0.000941" rpy="0 0 0"/>
+      <mass value="1.299"/>
+      <inertia ixx="0.0013873" ixy="1.63E-05" ixz="-1E-06" iyy="0.0009059" iyz="4.24E-05" izz="0.0009196"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_pitch_joint" type="revolute">
+    <origin xyz="0 -0.06445 -0.1027" rpy="0 -0.34907 0"/>
+    <parent link="pelvis"/>
+    <child link="right_hip_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.35" upper="3.05" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_roll_link">
+    <inertial>
+      <origin xyz="0.024757 0.001036 -0.086323" rpy="0 0 0"/>
+      <mass value="1.446"/>
+      <inertia ixx="0.0022702" ixy="3.7E-06" ixz="-0.0003789" iyy="0.002304" iyz="1.8E-05" izz="0.0016001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_roll_joint" type="revolute">
+    <origin xyz="0 -0.0523 0" rpy="0 0 0"/>
+    <parent link="right_hip_pitch_link"/>
+    <child link="right_hip_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.53" upper="0.26" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_hip_yaw_link">
+    <inertial>
+      <origin xyz="-0.053554 0.011477 -0.14067" rpy="0 0 0"/>
+      <mass value="2.052"/>
+      <inertia ixx="0.0087264" ixy="0.0004402" ixz="-0.0036676" iyy="0.011374" iyz="0.0006654" izz="0.004279"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_hip_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_hip_yaw_joint" type="revolute">
+    <origin xyz="0.01966 0.0012139 -0.1241" rpy="0 0 0"/>
+    <parent link="right_hip_roll_link"/>
+    <child link="right_hip_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.75" upper="2.75" effort="88" velocity="32"/>
+  </joint>
+  <link name="right_knee_link">
+    <inertial>
+      <origin xyz="0.005505 -0.006534 -0.116629" rpy="0 0 0"/>
+      <mass value="2.252"/>
+      <inertia ixx="0.012443837" ixy="-0.000053496" ixz="-0.000437641" iyy="0.012674902" iyz="0.000682499" izz="0.001986501"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_knee_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.007 -0.005 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.015" length="0.15"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_knee_joint" type="revolute">
+    <origin xyz="-0.078292 0.0017335 -0.177225" rpy="0 0.5096 0"/>
+    <parent link="right_hip_yaw_link"/>
+    <child link="right_knee_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.33489" upper="2.5449" effort="139" velocity="20"/>
+  </joint>
+  <link name="right_ankle_pitch_link">
+    <inertial>
+      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
+      <mass value="0.074"/>
+      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_ankle_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_ankle_pitch_joint" type="revolute">
+    <origin xyz="0 -0.0040687 -0.30007" rpy="0 -0.16053 0"/>
+    <parent link="right_knee_link"/>
+    <child link="right_ankle_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.68" upper="0.73" effort="40" velocity="53"/>
+  </joint>
+  <link name="right_ankle_roll_link">
+    <inertial>
+      <origin xyz="0.024762 -2E-05 -0.012526" rpy="0 0 0"/>
+      <mass value="0.391"/>
+      <inertia ixx="0.0001552" ixy="1E-07" ixz="7.51E-05" iyy="0.0010657" iyz="-1E-07" izz="0.001098"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_ankle_roll_link.STL"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+      <origin xyz="-0.06 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="-0.06 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.13 -0.02 -0.03" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="right_ankle_roll_joint" type="revolute">
+    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
+    <parent link="right_ankle_pitch_link"/>
+    <child link="right_ankle_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-0.2618" upper="0.2618" effort="40" velocity="53"/>
+  </joint>
+
+  <!-- Torso -->
+  <link name="torso_link">
+    <inertial>
+      <origin xyz="0.00197122283 0.00266902037 0.16936510960" rpy="0 0 0"/>
+      <mass value="6.33959811"/>
+      <inertia ixx="0.06037082109027" ixy="-0.00010168407055" ixz="0.00069845018092" iyy="0.04355153796551" iyz="0.00034366422728" izz="0.03339981866010"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/torso_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/torso_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="torso_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="pelvis"/>
+    <child link="torso_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
+  </joint>
+
+  <!-- Head -->
+  <link name="head_link">
+    <inertial>
+      <origin xyz="0.00138066852 0.00028430950 0.42034187824" rpy="0 0 0"/>
+      <mass value="1.17976522"/>
+      <inertia ixx="0.00543236042361" ixy="0.00000140137425" ixz="0.00034554752228" iyy="0.00552885306699" iyz="0.00001501216392" izz="0.00165378108136"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/head_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/head_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="head_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="head_link"/>
+  </joint>
+
+  <!-- L ARM -->
+  <link name="left_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="-9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 0.10396 0.25928" rpy="0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="left_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_roll_joint" type="fixed">
+    <origin xyz="0 0.052 0" rpy="-0.27925 0 0"/>
+    <parent link="left_shoulder_pitch_link"/>
+    <child link="left_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-1.5882" upper="2.2515" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 -0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="-4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 0.0062424 -0.1032" rpy="0 0 0"/>
+    <parent link="left_shoulder_roll_link"/>
+    <child link="left_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="left_shoulder_yaw_link"/>
+    <child link="left_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="left_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="-1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_pitch_link"/>
+    <child link="left_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- R ARM -->
+  <link name="right_shoulder_pitch_link">
+    <inertial>
+      <origin xyz="-0.001431 -0.048811 0.001304" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0004614" ixy="9.3E-06" ixz="1E-05" iyy="0.0004146" iyz="-5.5E-06" izz="0.0004416"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_shoulder_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_shoulder_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_pitch_joint" type="fixed">
+    <origin xyz="-0.0025 -0.10396 0.25928" rpy="-0.27925 0 0"/>
+    <parent link="torso_link"/>
+    <child link="right_shoulder_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.9671" upper="2.7925" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_roll_link">
+    <inertial>
+      <origin xyz="-0.003415 -0.006955 -0.064598" rpy="0 0 0"/>
+      <mass value="0.642"/>
+      <inertia ixx="0.0006159" ixy="0" ixz="-5.6E-06" iyy="0.0006835" iyz="-2.1E-06" izz="0.000373"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_shoulder_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.03" length="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_roll_joint" type="fixed">
+    <origin xyz="0 -0.052 0" rpy="0.27925 0 0"/>
+    <parent link="right_shoulder_pitch_link"/>
+    <child link="right_shoulder_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.2515" upper="1.5882" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_shoulder_yaw_link">
+    <inertial>
+      <origin xyz="0.000375 0.00444 -0.072374" rpy="0 0 0"/>
+      <mass value="0.713"/>
+      <inertia ixx="0.0009699" ixy="-7.1E-06" ixz="7.8E-06" iyy="0.0009691" iyz="4.49E-05" izz="0.0003826"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_shoulder_yaw_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_shoulder_yaw_joint" type="fixed">
+    <origin xyz="-0.00354 -0.0062424 -0.1032" rpy="0 0 0"/>
+    <parent link="right_shoulder_roll_link"/>
+    <child link="right_shoulder_yaw_link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.618" upper="2.618" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_pitch_link">
+    <inertial>
+      <origin xyz="0.064497 -0.002873 0" rpy="0 0 0"/>
+      <mass value="0.601"/>
+      <inertia ixx="0.0002845" ixy="-8.06E-05" ixz="9E-09" iyy="0.0004647" iyz="-5E-09" izz="0.0004712"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_elbow_pitch_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_elbow_pitch_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_pitch_joint" type="fixed">
+    <origin xyz="0 -0.00189 -0.0855" rpy="0 0 0"/>
+    <parent link="right_shoulder_yaw_link"/>
+    <child link="right_elbow_pitch_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-0.2268" upper="3.4208" effort="21" velocity="53"/>
+  </joint>
+  <link name="right_elbow_roll_link">
+    <inertial>
+      <origin xyz="0.081811 -0.001454 0.001" rpy="0 0 0"/>
+      <mass value="0.227"/>
+      <inertia ixx="0.0001093" ixy="1.55E-05" ixz="-2.91E-05" iyy="0.000977" iyz="-1E-06" izz="0.0009676"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_elbow_roll_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_elbow_roll_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_elbow_roll_joint" type="fixed">
+    <origin xyz="0.1 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_pitch_link"/>
+    <child link="right_elbow_roll_link"/>
+    <axis xyz="1 0 0"/>
+    <limit lower="-2.0943" upper="2.0943" effort="21" velocity="53"/>
+  </joint>
+
+  <!-- LOGO -->
+  <joint name="logo_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="logo_link"/>
+  </joint>
+  <link name="logo_link">
+    <inertial>
+      <origin xyz="-0.00306556489 0.00005625032 0.24526459523" rpy="0 0 0"/>
+      <mass value="0.001"/>
+      <inertia ixx="0.00000089650105" ixy="0.00000007028325" ixz="0.00000002735427" iyy="0.00000644846110" iyz="0.00000000029738" izz="0.00000688611155"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/logo_link.STL"/>
+      </geometry>
+      <material name="dark">
+        <color rgba="0.2 0.2 0.2 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/logo_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- IMU -->
+  <link name="imu_link"></link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="-0.04233868314 0.00166 0.152067" rpy="0 0 0"/>
+    <parent link="torso_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- Gripper -->
+  <joint name="left_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="left_elbow_roll_link"/>
+    <child link="left_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="left_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="-0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="left_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 -0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="-0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="left_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 -0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="left_zero_link"/>
+    <child link="left_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1" upper="1.2"/>
+  </joint>
+  <link name="left_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 -0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="-0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.001 -0.032 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.03 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_two_joint" type="fixed">
+    <origin xyz="0 -0.0431 0" rpy="0 0 0"/>
+    <parent link="left_one_link"/>
+    <child link="left_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="left_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 -0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="-0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_three_joint" type="fixed">
+    <origin xyz="0.085 0.004 0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_four_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_three_link"/>
+    <child link="left_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_five_joint" type="fixed">
+    <origin xyz="0.085 0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="left_palm_link"/>
+    <child link="left_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0.3"/>
+  </joint>
+  <link name="left_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 -0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="-0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_six_joint" type="fixed">
+    <origin xyz="0.0471 -0.0036 0" rpy="0 0 0"/>
+    <parent link="left_five_link"/>
+    <child link="left_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="left_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="-0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="-0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/left_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+  <joint name="right_palm_joint" type="fixed">
+    <origin xyz="0.120 0 0" rpy="0 0 0"/>
+    <parent link="right_elbow_roll_link"/>
+    <child link="right_palm_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="right_palm_link">
+    <inertial>
+      <origin xyz="0.05578422502 -0.00148509387 -0.00032670143" rpy="0 0 0"/>
+      <mass value="0.28126043"/>
+      <inertia ixx="0.00017638101047" ixy="0.00000652171140" ixz="0.00000021301775" iyy="0.00031084250366" iyz="-0.00000000708018" izz="0.00018939350451"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_palm_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_palm_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_zero_joint" type="fixed">
+    <origin xyz="0.050 0 0" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_zero_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.7" velocity="23" lower="-0.523598" upper="0.523598"/>
+  </joint>
+  <link name="right_zero_link">
+    <inertial>
+      <origin xyz="-0.02604664462 0.00877511180 0.00001086048" rpy="0 0 0"/>
+      <mass value="0.05098927"/>
+      <inertia ixx="0.00000679821860" ixy="0.00000098281498" ixz="-0.00000000406407" iyy="0.00000501631802" iyz="-0.00000002833450" izz="0.00000868017156"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_zero_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_zero_link.STL"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="right_one_joint" type="fixed">
+    <origin xyz="-0.02652502957 0.0188 -0.00005" rpy="0 0 0"/>
+    <parent link="right_zero_link"/>
+    <child link="right_one_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.2" upper="1"/>
+  </joint>
+  <link name="right_one_link">
+    <inertial>
+      <origin xyz="-0.00181538145 0.03270571529 -0.00018587117" rpy="0 0 0"/>
+      <mass value="0.04776204"/>
+      <inertia ixx="0.00000800634866" ixy="0.00000020811350" ixz="-0.00000004687654" iyy="0.00000366333777" iyz="-0.00000005133915" izz="0.00000785913677"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_one_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.001 0.032 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.02 0.03 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_two_joint" type="fixed">
+    <origin xyz="0 0.0431 0" rpy="0 0 0"/>
+    <parent link="right_one_link"/>
+    <child link="right_two_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-1.84" upper="0"/>
+  </joint>
+  <link name="right_two_link">
+    <inertial>
+      <origin xyz="-0.00135257289 0.02375910756 -0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000254698095" ixy="0.00000010086310" ixz="-0.00000000325710" iyy="0.00000094744556" iyz="-0.00000003605469" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_two_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_two_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_three_joint" type="fixed">
+    <origin xyz="0.085 -0.004 0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_three_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_three_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_three_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_three_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_four_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_three_link"/>
+    <child link="right_four_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_four_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_four_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_four_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_five_joint" type="fixed">
+    <origin xyz="0.085 -0.004 -0.02395" rpy="0 0 0"/>
+    <parent link="right_palm_link"/>
+    <child link="right_five_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="-0.3" upper="1.84"/>
+  </joint>
+  <link name="right_five_link">
+    <inertial>
+      <origin xyz="0.03625612321 0.00157250459 0.00026989918" rpy="0 0 0"/>
+      <mass value="0.04884689"/>
+      <inertia ixx="0.00000383218469" ixy="-0.00000003239984" ixz="0.00000001923377" iyy="0.00000890974599" iyz="0.00000004429800" izz="0.00000865612355"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_five_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_five_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="right_six_joint" type="fixed">
+    <origin xyz="0.0471 0.0036 0" rpy="0 0 0"/>
+    <parent link="right_five_link"/>
+    <child link="right_six_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.7" velocity="23" lower="0" upper="1.84"/>
+  </joint>
+  <link name="right_six_link">
+    <inertial>
+      <origin xyz="0.02375910756 -0.00135257289 0.00011966865" rpy="0 0 0"/>
+      <mass value="0.01385835"/>
+      <inertia ixx="0.00000094744556" ixy="0.00000010086310" ixz="0.00000003605469" iyy="0.00000254698095" iyz="0.00000000325710" izz="0.00000204288618"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_six_link.STL"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.7 0.7 0.7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/right_six_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
\ No newline at end of file