From 1a3e6489c06f551090b3ad0e617d791b9b84a38a Mon Sep 17 00:00:00 2001
From: "yecheng.shao" <shaoyecheng@zju.edu.cn>
Date: Fri, 19 Jan 2024 22:01:49 +0800
Subject: [PATCH] update inertial params and add _isaacgym files

---
 robots/h1_description/README.md               |    4 +-
 robots/h1_description/urdf/h1.urdf            |   32 +-
 robots/h1_description/urdf/h1_isaacgym.urdf   | 1205 +++++++
 robots/h1_description/urdf/h1_with_hand.urdf  |  351 +--
 .../urdf/h1_with_hand_isaacgym.urdf           | 2759 +++++++++++++++++
 5 files changed, 4150 insertions(+), 201 deletions(-)
 create mode 100644 robots/h1_description/urdf/h1_isaacgym.urdf
 create mode 100644 robots/h1_description/urdf/h1_with_hand_isaacgym.urdf

diff --git a/robots/h1_description/README.md b/robots/h1_description/README.md
index 67e89b3..34fb3ef 100644
--- a/robots/h1_description/README.md
+++ b/robots/h1_description/README.md
@@ -5,13 +5,13 @@
 This package includes a streamlined robot description (URDF & MJCF) for the [H1 Humanoid
 Robot](https://www.unitree.com/h1), developed by [Unitree Robotics](https://www.unitree.com/).
 
-The file `urdf/h1.urdf` is description for H1 without hands, and `urdf/h1_with_hand.urdf` for H1 with hands.
+The file `urdf/h1.urdf` is description for H1 without hands, and `urdf/h1_with_hand.urdf` for H1 with dexterous hands. For IsaacGym users, please the file with `_isaacgym.urdf` to avoid some visualization issues.
 
 <p align="center">
   <img src="doc/H1.png" width="500"/>
 </p>
 
-H1 Humanoid have 19 joints:
+Basic H1 Humanoid have 19 joints:
 
 ```text
 root [⚓] => /pelvis/
diff --git a/robots/h1_description/urdf/h1.urdf b/robots/h1_description/urdf/h1.urdf
index b44cf90..3a5975c 100644
--- a/robots/h1_description/urdf/h1.urdf
+++ b/robots/h1_description/urdf/h1.urdf
@@ -294,17 +294,17 @@ Stephen Brawner (brawner@gmail.com)
     name="left_ankle_link">
     <inertial>
       <origin
-        xyz="0.048568 0 -0.045609"
+        xyz="0.042575 -0.000001 -0.044672"
         rpy="0 0 0" />
       <mass
-        value="0.552448" />
+        value="0.474" />
       <inertia
-        ixx="0.000156"
-        ixy="0"
-        ixz="0.000143"
-        iyy="0.003620"
-        iyz="0"
-        izz="0.003551" />
+        ixx="0.000159668"
+        ixy="-0.000000005"
+        ixz="0.000141063"
+        iyy="0.002900286"
+        iyz="0.000000014"
+        izz="0.002805438" />
     </inertial>
     <visual>
       <origin
@@ -584,17 +584,17 @@ Stephen Brawner (brawner@gmail.com)
     name="right_ankle_link">
     <inertial>
       <origin
-        xyz="0.048568 0 -0.045609"
+        xyz="0.042575 0.000001 -0.044672"
         rpy="0 0 0" />
       <mass
-        value="0.552448" />
+        value="0.474" />
       <inertia
-        ixx="0.000156"
-        ixy="0"
-        ixz="0.000143"
-        iyy="0.003620"
-        iyz="0"
-        izz="0.003551" />
+        ixx="0.000159668"
+        ixy="0.000000005"
+        ixz="0.000141063"
+        iyy="0.002900286"
+        iyz="-0.000000014"
+        izz="0.002805438" />
     </inertial>
     <visual>
       <origin
diff --git a/robots/h1_description/urdf/h1_isaacgym.urdf b/robots/h1_description/urdf/h1_isaacgym.urdf
new file mode 100644
index 0000000..70dcddd
--- /dev/null
+++ b/robots/h1_description/urdf/h1_isaacgym.urdf
@@ -0,0 +1,1205 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by
+Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="h1_description">
+  <mujoco>
+    <compiler meshdir="../meshes" discardvisual="false" />
+  </mujoco>
+
+
+  <!-- [CAUTION] uncomment when convert to mujoco -->
+  <!-- <link name="world"></link>
+  <joint name="floating_base_joint" type="floating">
+    <parent link="world" />
+    <child link="pelvis" />
+  </joint> -->
+
+
+  <link
+    name="pelvis">
+    <inertial>
+      <origin
+        xyz="-0.0002 4E-05 -0.04522"
+        rpy="0 0 0" />
+      <mass
+        value="5.39" />
+      <inertia
+        ixx="0.044582"
+        ixy="8.7034E-05"
+        ixz="-1.9893E-05"
+        iyy="0.0082464"
+        iyz="4.021E-06"
+        izz="0.049021" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/pelvis.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/pelvis.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="left_hip_yaw_link">
+    <inertial>
+      <origin
+        xyz="-0.04923 0.0001 0.0072"
+        rpy="0 0 0" />
+      <mass
+        value="2.244" />
+      <inertia
+        ixx="0.0025731"
+        ixy="9.159E-06"
+        ixz="-0.00051948"
+        iyy="0.0030444"
+        iyz="1.949E-06"
+        izz="0.0022883" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <!-- [CAUTION] use cylinder for hip_yaw_link -->
+    <collision>
+      <origin
+        xyz="-0.067 0 0"
+        rpy="0 1.57075 0" />
+      <geometry>
+        <cylinder length="0.07" radius="0.06" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hip_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.0875 -0.1742"
+      rpy="0 0 0" />
+    <parent
+      link="pelvis" />
+    <child
+      link="left_hip_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_hip_roll_link">
+    <inertial>
+      <origin
+        xyz="-0.0058 -0.00319 -9E-05"
+        rpy="0 0 0" />
+      <mass
+        value="2.232" />
+      <inertia
+        ixx="0.0020603"
+        ixy="3.2115E-05"
+        ixz="2.878E-06"
+        iyy="0.0022482"
+        iyz="-7.813E-06"
+        izz="0.0024323" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hip_roll_joint"
+    type="revolute">
+    <origin
+      xyz="0.039468 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="left_hip_yaw_link" />
+    <child
+      link="left_hip_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_hip_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.00746 -0.02346 -0.08193"
+        rpy="0 0 0" />
+      <mass
+        value="4.152" />
+      <inertia
+        ixx="0.082618"
+        ixy="-0.00066654"
+        ixz="0.0040725"
+        iyy="0.081579"
+        iyz="0.0072024"
+        izz="0.0060081" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hip_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.11536 0"
+      rpy="0 0 0" />
+    <parent
+      link="left_hip_roll_link" />
+    <child
+      link="left_hip_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.57"
+      upper="1.57"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_knee_link">
+    <inertial>
+      <origin
+        xyz="-0.00136 -0.00512 -0.1384"
+        rpy="0 0 0" />
+      <mass
+        value="1.721" />
+      <inertia
+        ixx="0.012205"
+        ixy="-6.8431E-05"
+        ixz="0.0010862"
+        iyy="0.012509"
+        iyz="0.00022549"
+        izz="0.0020629" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_knee_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_knee_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_knee_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="left_hip_pitch_link" />
+    <child
+      link="left_knee_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.26"
+      upper="2.05"
+      effort="300"
+      velocity="14" />
+  </joint>
+  <link
+    name="left_ankle_link">
+    <inertial>
+      <origin
+        xyz="0.042575 -0.000001 -0.044672"
+        rpy="0 0 0" />
+      <mass
+        value="0.474" />
+      <inertia
+        ixx="0.000159668"
+        ixy="-0.000000005"
+        ixz="0.000141063"
+        iyy="0.002900286"
+        iyz="0.000000014"
+        izz="0.002805438" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_ankle_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_ankle_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_ankle_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="left_knee_link" />
+    <child
+      link="left_ankle_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.87"
+      upper="0.52"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="right_hip_yaw_link">
+    <inertial>
+      <origin
+        xyz="-0.04923 -0.0001 0.0072"
+        rpy="0 0 0" />
+      <mass
+        value="2.244" />
+      <inertia
+        ixx="0.0025731"
+        ixy="-9.159E-06"
+        ixz="-0.00051948"
+        iyy="0.0030444"
+        iyz="-1.949E-06"
+        izz="0.0022883" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <!-- [CAUTION] use cylinder for hip_yaw_link -->
+    <collision>
+      <origin
+        xyz="-0.067 0 0"
+        rpy="0 1.57075 0" />
+      <geometry>
+        <cylinder length="0.07" radius="0.06" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hip_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.0875 -0.1742"
+      rpy="0 0 0" />
+    <parent
+      link="pelvis" />
+    <child
+      link="right_hip_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="right_hip_roll_link">
+    <inertial>
+      <origin
+        xyz="-0.0058 0.00319 -9E-05"
+        rpy="0 0 0" />
+      <mass
+        value="2.232" />
+      <inertia
+        ixx="0.0020603"
+        ixy="-3.2115E-05"
+        ixz="2.878E-06"
+        iyy="0.0022482"
+        iyz="7.813E-06"
+        izz="0.0024323" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hip_roll_joint"
+    type="revolute">
+    <origin
+      xyz="0.039468 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="right_hip_yaw_link" />
+    <child
+      link="right_hip_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="right_hip_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.00746 0.02346 -0.08193"
+        rpy="0 0 0" />
+      <mass
+        value="4.152" />
+      <inertia
+        ixx="0.082618"
+        ixy="0.00066654"
+        ixz="0.0040725"
+        iyy="0.081579"
+        iyz="-0.0072024"
+        izz="0.0060081" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hip_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.11536 0"
+      rpy="0 0 0" />
+    <parent
+      link="right_hip_roll_link" />
+    <child
+      link="right_hip_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.57"
+      upper="1.57"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="right_knee_link">
+    <inertial>
+      <origin
+        xyz="-0.00136 0.00512 -0.1384"
+        rpy="0 0 0" />
+      <mass
+        value="1.721" />
+      <inertia
+        ixx="0.012205"
+        ixy="6.8431E-05"
+        ixz="0.0010862"
+        iyy="0.012509"
+        iyz="-0.00022549"
+        izz="0.0020629" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_knee_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_knee_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_knee_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="right_hip_pitch_link" />
+    <child
+      link="right_knee_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.26"
+      upper="2.05"
+      effort="300"
+      velocity="14" />
+  </joint>
+  <link
+    name="right_ankle_link">
+    <inertial>
+      <origin
+        xyz="0.042575 0.000001 -0.044672"
+        rpy="0 0 0" />
+      <mass
+        value="0.474" />
+      <inertia
+        ixx="0.000159668"
+        ixy="0.000000005"
+        ixz="0.000141063"
+        iyy="0.002900286"
+        iyz="-0.000000014"
+        izz="0.002805438" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_ankle_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_ankle_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_ankle_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="right_knee_link" />
+    <child
+      link="right_ankle_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.87"
+      upper="0.52"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="torso_link">
+    <inertial>
+      <origin
+        xyz="0.000489 0.002797 0.20484"
+        rpy="0 0 0" />
+      <mass
+        value="17.789" />
+      <inertia
+        ixx="0.4873"
+        ixy="-0.00053763"
+        ixz="0.0020276"
+        iyy="0.40963"
+        iyz="-0.00074582"
+        izz="0.12785" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/torso_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/torso_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="torso_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="pelvis" />
+    <child
+      link="torso_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.35"
+      upper="2.35"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_shoulder_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.005045 0.053657 -0.015715"
+        rpy="0 0 0" />
+      <mass
+        value="1.033" />
+      <inertia
+        ixx="0.0012985"
+        ixy="-1.7333E-05"
+        ixz="8.683E-06"
+        iyy="0.00087279"
+        iyz="3.9656E-05"
+        izz="0.00097338" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_shoulder_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0.0055 0.15535 0.42999"
+      rpy="0.43633 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="left_shoulder_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.87"
+      upper="2.87"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="left_shoulder_roll_link">
+    <inertial>
+      <origin
+        xyz="0.000679 0.00115 -0.094076"
+        rpy="0 0 0" />
+      <mass
+        value="0.793" />
+      <inertia
+        ixx="0.0015742"
+        ixy="2.298E-06"
+        ixz="-7.2265E-05"
+        iyy="0.0016973"
+        iyz="-6.3691E-05"
+        izz="0.0010183" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_shoulder_roll_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0055 0.0565 -0.0165"
+      rpy="-0.43633 0 0" />
+    <parent
+      link="left_shoulder_pitch_link" />
+    <child
+      link="left_shoulder_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.34"
+      upper="3.11"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="left_shoulder_yaw_link">
+    <inertial>
+      <origin
+        xyz="0.01365 0.002767 -0.16266"
+        rpy="0 0 0" />
+      <mass
+        value="0.839" />
+      <inertia
+        ixx="0.003664"
+        ixy="-1.0671E-05"
+        ixz="0.00034733"
+        iyy="0.0040789"
+        iyz="7.0213E-05"
+        izz="0.00066383" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_yaw_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_shoulder_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.1343"
+      rpy="0 0 0" />
+    <parent
+      link="left_shoulder_roll_link" />
+    <child
+      link="left_shoulder_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.3"
+      upper="4.45"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="left_elbow_link">
+    <inertial>
+      <origin
+        xyz="0.15908 -0.000144 -0.015776"
+        rpy="0 0 0" />
+      <mass
+        value="0.669" />
+      <inertia
+        ixx="0.00042388"
+        ixy="-3.6086E-05"
+        ixz="0.00029293"
+        iyy="0.0060062"
+        iyz="4.664E-06"
+        izz="0.0060023" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_elbow_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_elbow_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_elbow_joint"
+    type="revolute">
+    <origin
+      xyz="0.0185 0 -0.198"
+      rpy="0 0 0" />
+    <parent
+      link="left_shoulder_yaw_link" />
+    <child
+      link="left_elbow_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.25"
+      upper="2.61"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="right_shoulder_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.005045 -0.053657 -0.015715"
+        rpy="0 0 0" />
+      <mass
+        value="1.033" />
+      <inertia
+        ixx="0.0012985"
+        ixy="1.7333E-05"
+        ixz="8.683E-06"
+        iyy="0.00087279"
+        iyz="-3.9656E-05"
+        izz="0.00097338" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_shoulder_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0.0055 -0.15535 0.42999"
+      rpy="-0.43633 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="right_shoulder_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.87"
+      upper="2.87"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="right_shoulder_roll_link">
+    <inertial>
+      <origin
+        xyz="0.000679 -0.00115 -0.094076"
+        rpy="0 0 0" />
+      <mass
+        value="0.793" />
+      <inertia
+        ixx="0.0015742"
+        ixy="-2.298E-06"
+        ixz="-7.2265E-05"
+        iyy="0.0016973"
+        iyz="6.3691E-05"
+        izz="0.0010183" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_shoulder_roll_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0055 -0.0565 -0.0165"
+      rpy="0.43633 0 0" />
+    <parent
+      link="right_shoulder_pitch_link" />
+    <child
+      link="right_shoulder_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-3.11"
+      upper="0.34"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="right_shoulder_yaw_link">
+    <inertial>
+      <origin
+        xyz="0.01365 -0.002767 -0.16266"
+        rpy="0 0 0" />
+      <mass
+        value="0.839" />
+      <inertia
+        ixx="0.003664"
+        ixy="1.0671E-05"
+        ixz="0.00034733"
+        iyy="0.0040789"
+        iyz="-7.0213E-05"
+        izz="0.00066383" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_yaw_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_shoulder_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.1343"
+      rpy="0 0 0" />
+    <parent
+      link="right_shoulder_roll_link" />
+    <child
+      link="right_shoulder_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-4.45"
+      upper="1.3"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="right_elbow_link">
+    <inertial>
+      <origin
+        xyz="0.15908 0.000144 -0.015776"
+        rpy="0 0 0" />
+      <mass
+        value="0.669" />
+      <inertia
+        ixx="0.00042388"
+        ixy="3.6086E-05"
+        ixz="0.00029293"
+        iyy="0.0060062"
+        iyz="-4.664E-06"
+        izz="0.0060023" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_elbow_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1 0.1 0.1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_elbow_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_elbow_joint"
+    type="revolute">
+    <origin
+      xyz="0.0185 0 -0.198"
+      rpy="0 0 0" />
+    <parent
+      link="right_shoulder_yaw_link" />
+    <child
+      link="right_elbow_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.25"
+      upper="2.61"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link name="imu_link"></link>
+  <joint
+    name="imu_joint"
+    type="fixed">
+    <origin
+      xyz="-0.04452 -0.01891 0.27756"
+      rpy="0 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="imu_link" />
+  </joint>
+  <link
+    name="logo_link">
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/logo_link_gray.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+  </link>
+  <joint
+    name="logo_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="logo_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>
\ No newline at end of file
diff --git a/robots/h1_description/urdf/h1_with_hand.urdf b/robots/h1_description/urdf/h1_with_hand.urdf
index ffd73cf..bd0c4a7 100644
--- a/robots/h1_description/urdf/h1_with_hand.urdf
+++ b/robots/h1_description/urdf/h1_with_hand.urdf
@@ -4,17 +4,17 @@
     name="pelvis">
     <inertial>
       <origin
-        xyz="3.31722511855514E-06 2.5546944378684E-07 -0.0427174809482497"
+        xyz="-0.0002 4E-05 -0.04522"
         rpy="0 0 0" />
       <mass
-        value="2.46720698341788" />
+        value="5.39" />
       <inertia
-        ixx="0.0155794040240623"
-        ixy="-1.70900789186328E-07"
-        ixz="-2.64171934477548E-07"
-        iyy="0.00425456762413368"
-        iyz="1.32904382115621E-08"
-        izz="0.0177014785496157" />
+        ixx="0.044582"
+        ixy="8.7034E-05"
+        ixz="-1.9893E-05"
+        iyy="0.0082464"
+        iyz="4.021E-06"
+        izz="0.049021" />
     </inertial>
     <visual>
       <origin
@@ -44,17 +44,17 @@
     name="left_hip_yaw_link">
     <inertial>
       <origin
-        xyz="-0.0552998470465489 2.32727512759767E-06 0.0125466081929538"
+        xyz="-0.04923 0.0001 0.0072"
         rpy="0 0 0" />
       <mass
-        value="0.812221266540345" />
+        value="2.244" />
       <inertia
-        ixx="0.00181988449372547"
-        ixy="7.65213610747462E-08"
-        ixz="-0.000464190352912876"
-        iyy="0.00185317260162015"
-        iyz="3.49347421376249E-08"
-        izz="0.00128996301196461" />
+        ixx="0.0025731"
+        ixy="9.159E-06"
+        ixz="-0.00051948"
+        iyy="0.0030444"
+        iyz="1.949E-06"
+        izz="0.0022883" />
     </inertial>
     <visual>
       <origin
@@ -96,23 +96,23 @@
       lower="-0.43"
       upper="0.43"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="left_hip_roll_link">
     <inertial>
       <origin
-        xyz="-0.00611009201879201 -0.012911613784405 2.28525148823766E-06"
+        xyz="-0.0058 -0.00319 -9E-05"
         rpy="0 0 0" />
       <mass
-        value="0.760397923884604" />
+        value="2.232" />
       <inertia
-        ixx="0.000941924987770898"
-        ixy="5.55416617763835E-05"
-        ixz="8.29578746733772E-09"
-        iyy="0.00135078288427786"
-        iyz="6.80474575168109E-08"
-        izz="0.00107007199090097" />
+        ixx="0.0020603"
+        ixy="3.2115E-05"
+        ixz="2.878E-06"
+        iyy="0.0022482"
+        iyz="-7.813E-06"
+        izz="0.0024323" />
     </inertial>
     <visual>
       <origin
@@ -142,7 +142,7 @@
     name="left_hip_roll_joint"
     type="revolute">
     <origin
-      xyz="0.03946772787 0 0"
+      xyz="0.039468 0 0"
       rpy="0 0 0" />
     <parent
       link="left_hip_yaw_link" />
@@ -154,23 +154,23 @@
       lower="-0.43"
       upper="0.43"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="left_hip_pitch_link">
     <inertial>
       <origin
-        xyz="0.0124359528977009 -0.0177785309930213 -0.102715684537467"
+        xyz="0.00746 -0.02346 -0.08193"
         rpy="0 0 0" />
       <mass
-        value="2.51390562613671" />
+        value="4.152" />
       <inertia
-        ixx="0.0462339754048383"
-        ixy="-0.000418263062499571"
-        ixz="0.00333498421418784"
-        iyy="0.0459435477825226"
-        iyz="0.00399425974700521"
-        izz="0.00437682290745374" />
+        ixx="0.082618"
+        ixy="-0.00066654"
+        ixz="0.0040725"
+        iyy="0.081579"
+        iyz="0.0072024"
+        izz="0.0060081" />
     </inertial>
     <visual>
       <origin
@@ -200,7 +200,7 @@
     name="left_hip_pitch_joint"
     type="revolute">
     <origin
-      xyz="0 0.11536462424 0"
+      xyz="0 0.11536 0"
       rpy="0 0 0" />
     <parent
       link="left_hip_roll_link" />
@@ -212,23 +212,23 @@
       lower="-1.57"
       upper="0.56"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="left_knee_link">
     <inertial>
       <origin
-        xyz="0.0111532652986465 -0.00210148358371862 -0.16436342641949"
+        xyz="-0.00136 -0.00512 -0.1384"
         rpy="0 0 0" />
       <mass
-        value="1.38784062515733" />
+        value="1.721" />
       <inertia
-        ixx="0.0176588759617023"
-        ixy="-6.81694760107566E-05"
-        ixz="0.000703347720715395"
-        iyy="0.0180835867027155"
-        iyz="0.000143259566014091"
-        izz="0.00127125869292153" />
+        ixx="0.012205"
+        ixy="-6.8431E-05"
+        ixz="0.0010862"
+        iyy="0.012509"
+        iyz="0.00022549"
+        izz="0.0020629" />
     </inertial>
     <visual>
       <origin
@@ -270,7 +270,7 @@
       lower="-0.26"
       upper="2.05"
       effort="300"
-      velocity="0" />
+      velocity="14" />
   </joint>
   <link
     name="left_ankle_link">
@@ -328,23 +328,23 @@
       lower="-0.87"
       upper="0.52"
       effort="40"
-      velocity="0" />
+      velocity="9" />
   </joint>
   <link
     name="right_hip_yaw_link">
     <inertial>
       <origin
-        xyz="-0.0552998734981944 -2.19116210505022E-06 0.0125466010739156"
+        xyz="-0.04923 -0.0001 0.0072"
         rpy="0 0 0" />
       <mass
-        value="0.812220994092406" />
+        value="2.244" />
       <inertia
-        ixx="0.00181988459757284"
-        ixy="-7.92404310211978E-08"
-        ixz="-0.000464190519818566"
-        iyy="0.00185317301794793"
-        iyz="-3.65409881564551E-08"
-        izz="0.00128996256133052" />
+        ixx="0.0025731"
+        ixy="-9.159E-06"
+        ixz="-0.00051948"
+        iyy="0.0030444"
+        iyz="-1.949E-06"
+        izz="0.0022883" />
     </inertial>
     <visual>
       <origin
@@ -386,23 +386,23 @@
       lower="-0.43"
       upper="0.43"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="right_hip_roll_link">
     <inertial>
       <origin
-        xyz="-0.00611020944997454 0.0129115573573222 2.30553074595519E-06"
+        xyz="-0.0058 0.00319 -9E-05"
         rpy="0 0 0" />
       <mass
-        value="0.760400057805762" />
+        value="2.232" />
       <inertia
-        ixx="0.000941929527765284"
-        ixy="-5.55428898773058E-05"
-        ixz="8.89614878164627E-09"
-        iyy="0.00135078866650044"
-        iyz="-6.86505933741414E-08"
-        izz="0.00107007557612766" />
+        ixx="0.0020603"
+        ixy="-3.2115E-05"
+        ixz="2.878E-06"
+        iyy="0.0022482"
+        iyz="7.813E-06"
+        izz="0.0024323" />
     </inertial>
     <visual>
       <origin
@@ -432,7 +432,7 @@
     name="right_hip_roll_joint"
     type="revolute">
     <origin
-      xyz="0.03946772787 0 0"
+      xyz="0.039468 0 0"
       rpy="0 0 0" />
     <parent
       link="right_hip_yaw_link" />
@@ -444,23 +444,23 @@
       lower="-0.43"
       upper="0.43"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="right_hip_pitch_link">
     <inertial>
       <origin
-        xyz="0.0124359930114926 0.0177784573102787 -0.102716658926343"
+        xyz="0.00746 0.02346 -0.08193"
         rpy="0 0 0" />
       <mass
-        value="2.51391221514723" />
+        value="4.152" />
       <inertia
-        ixx="0.0462347590313498"
-        ixy="0.000418268143106936"
-        ixz="0.00333508240549552"
-        iyy="0.0459443173081576"
-        iyz="-0.00399432390662422"
-        izz="0.00437681495700316" />
+        ixx="0.082618"
+        ixy="0.00066654"
+        ixz="0.0040725"
+        iyy="0.081579"
+        iyz="-0.0072024"
+        izz="0.0060081" />
     </inertial>
     <visual>
       <origin
@@ -490,7 +490,7 @@
     name="right_hip_pitch_joint"
     type="revolute">
     <origin
-      xyz="0 -0.11536462424 0"
+      xyz="0 -0.11536 0"
       rpy="0 0 0" />
     <parent
       link="right_hip_roll_link" />
@@ -502,23 +502,23 @@
       lower="-1.57"
       upper="0.56"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="right_knee_link">
     <inertial>
       <origin
-        xyz="0.0111532054494979 0.00210147201777963 -0.164362889115614"
+        xyz="-0.00136 0.00512 -0.1384"
         rpy="0 0 0" />
       <mass
-        value="1.38785091435145" />
+        value="1.721" />
       <inertia
-        ixx="0.0176589354193338"
-        ixy="6.81692212087034E-05"
-        ixz="0.00070335289606269"
-        iyy="0.0180836459434297"
-        iyz="-0.000143258860211554"
-        izz="0.00127126016185647" />
+        ixx="0.012205"
+        ixy="6.8431E-05"
+        ixz="0.0010862"
+        iyy="0.012509"
+        iyz="-0.00022549"
+        izz="0.0020629" />
     </inertial>
     <visual>
       <origin
@@ -560,7 +560,7 @@
       lower="-0.26"
       upper="2.05"
       effort="300"
-      velocity="0" />
+      velocity="14" />
   </joint>
   <link
     name="right_ankle_link">
@@ -618,23 +618,23 @@
       lower="-0.87"
       upper="0.52"
       effort="40"
-      velocity="0" />
+      velocity="9" />
   </joint>
   <link
     name="torso_link">
     <inertial>
       <origin
-        xyz="0.0019209642948768 -3.63188229712087E-07 0.269096889041749"
+        xyz="0.000489 0.002797 0.20484"
         rpy="0 0 0" />
       <mass
-        value="15.3277728264048" />
+        value="17.789" />
       <inertia
-        ixx="0.422617821814499"
-        ixy="-1.32755992528604E-06"
-        ixz="-0.0107840988303801"
-        iyy="0.387119328646928"
-        iyz="1.43678896468394E-06"
-        izz="0.0895823037542743" />
+        ixx="0.4873"
+        ixy="-0.00053763"
+        ixz="0.0020276"
+        iyy="0.40963"
+        iyz="-0.00074582"
+        izz="0.12785" />
     </inertial>
     <visual>
       <origin
@@ -676,23 +676,23 @@
       lower="-2.35"
       upper="2.35"
       effort="200"
-      velocity="0" />
+      velocity="23" />
   </joint>
   <link
     name="left_shoulder_pitch_link">
     <inertial>
       <origin
-        xyz="-0.00431423428819262 0.0545680722429941 -0.0158057885614539"
+        xyz="0.005045 0.053657 -0.015715"
         rpy="0 0 0" />
       <mass
-        value="0.39711525759175" />
+        value="1.033" />
       <inertia
-        ixx="0.000426933371038538"
-        ixy="3.3131010585919E-06"
-        ixz="-1.12961756205576E-06"
-        iyy="0.000319447136982771"
-        iyz="1.32534140926153E-05"
-        izz="0.000344820669777049" />
+        ixx="0.0012985"
+        ixy="-1.7333E-05"
+        ixz="8.683E-06"
+        iyy="0.00087279"
+        iyz="3.9656E-05"
+        izz="0.00097338" />
     </inertial>
     <visual>
       <origin
@@ -722,8 +722,8 @@
     name="left_shoulder_pitch_joint"
     type="revolute">
     <origin
-      xyz="0.0055 0.155354297602023 0.429991895369114"
-      rpy="0.43633231299858 0 0" />
+      xyz="0.0055 0.15535 0.42999"
+      rpy="0.43633 0 0" />
     <parent
       link="torso_link" />
     <child
@@ -734,23 +734,23 @@
       lower="-2.87"
       upper="2.87"
       effort="40"
-      velocity="0" />
+      velocity="9" />
   </joint>
   <link
     name="left_shoulder_roll_link">
     <inertial>
       <origin
-        xyz="0.00506927781751245 -0.000350708976701253 -0.102229263710221"
+        xyz="0.000679 0.00115 -0.094076"
         rpy="0 0 0" />
       <mass
-        value="0.291989882010004" />
+        value="0.793" />
       <inertia
-        ixx="0.000420736177139627"
-        ixy="-1.38579766838052E-06"
-        ixz="-9.44887032649605E-05"
-        iyy="0.00045163585569499"
-        iyz="-4.71688243530896E-06"
-        izz="0.000327311700509138" />
+        ixx="0.0015742"
+        ixy="2.298E-06"
+        ixz="-7.2265E-05"
+        iyy="0.0016973"
+        iyz="-6.3691E-05"
+        izz="0.0010183" />
     </inertial>
     <visual>
       <origin
@@ -780,8 +780,8 @@
     name="left_shoulder_roll_joint"
     type="revolute">
     <origin
-      xyz="-0.0055 0.0565000000018586 -0.0165"
-      rpy="-0.43633231299858 0 0" />
+      xyz="-0.0055 0.0565 -0.0165"
+      rpy="-0.43633 0 0" />
     <parent
       link="left_shoulder_pitch_link" />
     <child
@@ -792,23 +792,23 @@
       lower="-0.34"
       upper="3.11"
       effort="40"
-      velocity="0" />
+      velocity="9" />
   </joint>
   <link
     name="left_shoulder_yaw_link">
     <inertial>
       <origin
-        xyz="0.00778389584984959 0.000767058526100817 -0.125431328972536"
+        xyz="0.01365 0.002767 -0.16266"
         rpy="0 0 0" />
       <mass
-        value="0.681412366138755" />
+        value="0.839" />
       <inertia
-        ixx="0.0034501083469846"
-        ixy="-3.72566385545378E-06"
-        ixz="0.000348455120219903"
-        iyy="0.00364220548154351"
-        iyz="3.07109813186911E-05"
-        izz="0.000431079833886041" />
+        ixx="0.003664"
+        ixy="-1.0671E-05"
+        ixz="0.00034733"
+        iyy="0.0040789"
+        iyz="7.0213E-05"
+        izz="0.00066383" />
     </inertial>
     <visual>
       <origin
@@ -849,8 +849,8 @@
     <limit
       lower="-1.3"
       upper="4.45"
-      effort="15"
-      velocity="0" />
+      effort="18"
+      velocity="20" />
   </joint>
   <link
     name="left_elbow_link">
@@ -907,8 +907,8 @@
     <limit
       lower="-1.25"
       upper="2.61"
-      effort="15"
-      velocity="0" />
+      effort="18"
+      velocity="20" />
   </joint>
   <link
     name="left_hand_link">
@@ -965,8 +965,8 @@
     <limit
       lower="-3.0543261909900767596164588448551"
       upper="3.0543261909900767596164588448551"
-      effort="7"
-      velocity="0" />
+      effort="6"
+      velocity="12" />
   </joint>
     <joint 
       name="L_base_link_joint" 
@@ -1701,17 +1701,17 @@
     name="right_shoulder_pitch_link">
     <inertial>
       <origin
-        xyz="-0.00431422175136645 -0.0545680787603574 -0.0158057806958861"
+        xyz="0.005045 -0.053657 -0.015715"
         rpy="0 0 0" />
       <mass
-        value="0.397115199412805" />
+        value="1.033" />
       <inertia
-        ixx="0.000426933390625267"
-        ixy="-3.31334388566935E-06"
-        ixz="-1.12956544864161E-06"
-        iyy="0.000319446999853806"
-        iyz="-1.32532524614685E-05"
-        izz="0.000344820853743014" />
+        ixx="0.0012985"
+        ixy="1.7333E-05"
+        ixz="8.683E-06"
+        iyy="0.00087279"
+        iyz="-3.9656E-05"
+        izz="0.00097338" />
     </inertial>
     <visual>
       <origin
@@ -1741,8 +1741,8 @@
     name="right_shoulder_pitch_joint"
     type="revolute">
     <origin
-      xyz="0.0055 -0.155354297602023 0.429991895369114"
-      rpy="-0.43633231299858 0 0" />
+      xyz="0.0055 -0.15535 0.42999"
+      rpy="-0.43633 0 0" />
     <parent
       link="torso_link" />
     <child
@@ -1753,23 +1753,23 @@
       lower="-2.87"
       upper="2.87"
       effort="40"
-      velocity="0" />
+      velocity="9" />
   </joint>
   <link
     name="right_shoulder_roll_link">
     <inertial>
       <origin
-        xyz="0.0050692009517695 0.000350652484244984 -0.102229219490617"
+        xyz="0.000679 -0.00115 -0.094076"
         rpy="0 0 0" />
       <mass
-        value="0.291991233598522" />
+        value="0.793" />
       <inertia
-        ixx="0.000420738020082048"
-        ixy="1.3867065503541E-06"
-        ixz="-9.4488534085745E-05"
-        iyy="0.000451637571048155"
-        iyz="4.71690087354637E-06"
-        izz="0.00032731250134499" />
+        ixx="0.0015742"
+        ixy="-2.298E-06"
+        ixz="-7.2265E-05"
+        iyy="0.0016973"
+        iyz="6.3691E-05"
+        izz="0.0010183" />
     </inertial>
     <visual>
       <origin
@@ -1799,8 +1799,8 @@
     name="right_shoulder_roll_joint"
     type="revolute">
     <origin
-      xyz="-0.0055 -0.0565000000018587 -0.0165"
-      rpy="0.43633231299858 0 0" />
+      xyz="-0.0055 -0.0565 -0.0165"
+      rpy="0.43633 0 0" />
     <parent
       link="right_shoulder_pitch_link" />
     <child
@@ -1811,23 +1811,23 @@
       lower="-3.11"
       upper="0.34"
       effort="40"
-      velocity="0" />
+      velocity="9" />
   </joint>
   <link
     name="right_shoulder_yaw_link">
     <inertial>
       <origin
-        xyz="0.00778534571932127 -0.000767745515027396 -0.125434157510562"
+        xyz="0.01365 -0.002767 -0.16266"
         rpy="0 0 0" />
       <mass
-        value="0.681426758035702" />
+        value="0.839" />
       <inertia
-        ixx="0.00345015704507685"
-        ixy="3.75079238462045E-06"
-        ixz="0.000348545200584949"
-        iyy="0.00364231385191047"
-        iyz="-3.07151563883643E-05"
-        izz="0.00043113614520676" />
+        ixx="0.003664"
+        ixy="1.0671E-05"
+        ixz="0.00034733"
+        iyy="0.0040789"
+        iyz="-7.0213E-05"
+        izz="0.00066383" />
     </inertial>
     <visual>
       <origin
@@ -1868,10 +1868,9 @@
     <limit
       lower="-4.45"
       upper="1.3"
-      effort="15"
-      velocity="0" />
+      effort="18"
+      velocity="20" />
   </joint>
-
   <link
     name="right_elbow_link">
     <inertial>
@@ -1927,8 +1926,8 @@
     <limit
       lower="-1.25"
       upper="2.61"
-      effort="15"
-      velocity="0" />
+      effort="18"
+      velocity="20" />
   </joint>
   <link
     name="right_hand_link">
@@ -1985,8 +1984,8 @@
     <limit
       lower="-3.0543261909900767596164588448551"
       upper="3.0543261909900767596164588448551"
-      effort="7"
-      velocity="0" />
+      effort="6"
+      velocity="12" />
   </joint>
 
     <joint 
@@ -2720,27 +2719,13 @@
   </joint>
   <link
     name="logo_link">
-    <inertial>
-      <origin
-        xyz="-3.11982502935774E-09 0.00121715353088297 0.383821850968879"
-        rpy="0 0 0" />
-      <mass
-        value="0.014618114050549" />
-      <inertia
-        ixx="1.19912416942028E-06"
-        ixy="1.09302002295816E-13"
-        ixz="4.27296305445089E-15"
-        iyy="7.5811401810277E-07"
-        iyz="1.59497688389296E-09"
-        izz="4.70855407007677E-07" />
-    </inertial>
     <visual>
       <origin
         xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://h1_description/meshes/logo_link_gray.dae" />
+          filename="package://h1_description/meshes/logo_link.STL" />
       </geometry>
       <material
         name="">
diff --git a/robots/h1_description/urdf/h1_with_hand_isaacgym.urdf b/robots/h1_description/urdf/h1_with_hand_isaacgym.urdf
new file mode 100644
index 0000000..900e119
--- /dev/null
+++ b/robots/h1_description/urdf/h1_with_hand_isaacgym.urdf
@@ -0,0 +1,2759 @@
+<robot
+  name="h1">
+  <link
+    name="pelvis">
+    <inertial>
+      <origin
+        xyz="-0.0002 4E-05 -0.04522"
+        rpy="0 0 0" />
+      <mass
+        value="5.39" />
+      <inertia
+        ixx="0.044582"
+        ixy="8.7034E-05"
+        ixz="-1.9893E-05"
+        iyy="0.0082464"
+        iyz="4.021E-06"
+        izz="0.049021" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/pelvis.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/pelvis.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="left_hip_yaw_link">
+    <inertial>
+      <origin
+        xyz="-0.04923 0.0001 0.0072"
+        rpy="0 0 0" />
+      <mass
+        value="2.244" />
+      <inertia
+        ixx="0.0025731"
+        ixy="9.159E-06"
+        ixz="-0.00051948"
+        iyy="0.0030444"
+        iyz="1.949E-06"
+        izz="0.0022883" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_yaw_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hip_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.0875 -0.1742"
+      rpy="0 0 0" />
+    <parent
+      link="pelvis" />
+    <child
+      link="left_hip_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_hip_roll_link">
+    <inertial>
+      <origin
+        xyz="-0.0058 -0.00319 -9E-05"
+        rpy="0 0 0" />
+      <mass
+        value="2.232" />
+      <inertia
+        ixx="0.0020603"
+        ixy="3.2115E-05"
+        ixz="2.878E-06"
+        iyy="0.0022482"
+        iyz="-7.813E-06"
+        izz="0.0024323" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hip_roll_joint"
+    type="revolute">
+    <origin
+      xyz="0.039468 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="left_hip_yaw_link" />
+    <child
+      link="left_hip_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_hip_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.00746 -0.02346 -0.08193"
+        rpy="0 0 0" />
+      <mass
+        value="4.152" />
+      <inertia
+        ixx="0.082618"
+        ixy="-0.00066654"
+        ixz="0.0040725"
+        iyy="0.081579"
+        iyz="0.0072024"
+        izz="0.0060081" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hip_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hip_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.11536 0"
+      rpy="0 0 0" />
+    <parent
+      link="left_hip_roll_link" />
+    <child
+      link="left_hip_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.57"
+      upper="0.56"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_knee_link">
+    <inertial>
+      <origin
+        xyz="-0.00136 -0.00512 -0.1384"
+        rpy="0 0 0" />
+      <mass
+        value="1.721" />
+      <inertia
+        ixx="0.012205"
+        ixy="-6.8431E-05"
+        ixz="0.0010862"
+        iyy="0.012509"
+        iyz="0.00022549"
+        izz="0.0020629" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_knee_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_knee_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_knee_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="left_hip_pitch_link" />
+    <child
+      link="left_knee_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.26"
+      upper="2.05"
+      effort="300"
+      velocity="14" />
+  </joint>
+  <link
+    name="left_ankle_link">
+    <inertial>
+      <origin
+        xyz="0.042575 -0.000001 -0.044672"
+        rpy="0 0 0" />
+      <mass
+        value="0.474" />
+      <inertia
+        ixx="0.000159668"
+        ixy="-0.000000005"
+        ixz="0.000141063"
+        iyy="0.002900286"
+        iyz="0.000000014"
+        izz="0.002805438" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_ankle_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_ankle_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_ankle_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="left_knee_link" />
+    <child
+      link="left_ankle_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.87"
+      upper="0.52"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="right_hip_yaw_link">
+    <inertial>
+      <origin
+        xyz="-0.04923 -0.0001 0.0072"
+        rpy="0 0 0" />
+      <mass
+        value="2.244" />
+      <inertia
+        ixx="0.0025731"
+        ixy="-9.159E-06"
+        ixz="-0.00051948"
+        iyy="0.0030444"
+        iyz="-1.949E-06"
+        izz="0.0022883" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_yaw_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hip_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.0875 -0.1742"
+      rpy="0 0 0" />
+    <parent
+      link="pelvis" />
+    <child
+      link="right_hip_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="right_hip_roll_link">
+    <inertial>
+      <origin
+        xyz="-0.0058 0.00319 -9E-05"
+        rpy="0 0 0" />
+      <mass
+        value="2.232" />
+      <inertia
+        ixx="0.0020603"
+        ixy="-3.2115E-05"
+        ixz="2.878E-06"
+        iyy="0.0022482"
+        iyz="7.813E-06"
+        izz="0.0024323" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hip_roll_joint"
+    type="revolute">
+    <origin
+      xyz="0.039468 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="right_hip_yaw_link" />
+    <child
+      link="right_hip_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.43"
+      upper="0.43"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="right_hip_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.00746 0.02346 -0.08193"
+        rpy="0 0 0" />
+      <mass
+        value="4.152" />
+      <inertia
+        ixx="0.082618"
+        ixy="0.00066654"
+        ixz="0.0040725"
+        iyy="0.081579"
+        iyz="-0.0072024"
+        izz="0.0060081" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hip_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hip_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0 -0.11536 0"
+      rpy="0 0 0" />
+    <parent
+      link="right_hip_roll_link" />
+    <child
+      link="right_hip_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.57"
+      upper="0.56"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="right_knee_link">
+    <inertial>
+      <origin
+        xyz="-0.00136 0.00512 -0.1384"
+        rpy="0 0 0" />
+      <mass
+        value="1.721" />
+      <inertia
+        ixx="0.012205"
+        ixy="6.8431E-05"
+        ixz="0.0010862"
+        iyy="0.012509"
+        iyz="-0.00022549"
+        izz="0.0020629" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_knee_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_knee_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_knee_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="right_hip_pitch_link" />
+    <child
+      link="right_knee_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.26"
+      upper="2.05"
+      effort="300"
+      velocity="14" />
+  </joint>
+  <link
+    name="right_ankle_link">
+    <inertial>
+      <origin
+        xyz="0.042575 0.000001 -0.044672"
+        rpy="0 0 0" />
+      <mass
+        value="0.474" />
+      <inertia
+        ixx="0.000159668"
+        ixy="0.000000005"
+        ixz="0.000141063"
+        iyy="0.002900286"
+        iyz="-0.000000014"
+        izz="0.002805438" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_ankle_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_ankle_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_ankle_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.4"
+      rpy="0 0 0" />
+    <parent
+      link="right_knee_link" />
+    <child
+      link="right_ankle_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-0.87"
+      upper="0.52"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="torso_link">
+    <inertial>
+      <origin
+        xyz="0.000489 0.002797 0.20484"
+        rpy="0 0 0" />
+      <mass
+        value="17.789" />
+      <inertia
+        ixx="0.4873"
+        ixy="-0.00053763"
+        ixz="0.0020276"
+        iyy="0.40963"
+        iyz="-0.00074582"
+        izz="0.12785" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/torso_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/torso_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="torso_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="pelvis" />
+    <child
+      link="torso_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.35"
+      upper="2.35"
+      effort="200"
+      velocity="23" />
+  </joint>
+  <link
+    name="left_shoulder_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.005045 0.053657 -0.015715"
+        rpy="0 0 0" />
+      <mass
+        value="1.033" />
+      <inertia
+        ixx="0.0012985"
+        ixy="-1.7333E-05"
+        ixz="8.683E-06"
+        iyy="0.00087279"
+        iyz="3.9656E-05"
+        izz="0.00097338" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_shoulder_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0.0055 0.15535 0.42999"
+      rpy="0.43633 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="left_shoulder_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.87"
+      upper="2.87"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="left_shoulder_roll_link">
+    <inertial>
+      <origin
+        xyz="0.000679 0.00115 -0.094076"
+        rpy="0 0 0" />
+      <mass
+        value="0.793" />
+      <inertia
+        ixx="0.0015742"
+        ixy="2.298E-06"
+        ixz="-7.2265E-05"
+        iyy="0.0016973"
+        iyz="-6.3691E-05"
+        izz="0.0010183" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_shoulder_roll_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0055 0.0565 -0.0165"
+      rpy="-0.43633 0 0" />
+    <parent
+      link="left_shoulder_pitch_link" />
+    <child
+      link="left_shoulder_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-0.34"
+      upper="3.11"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="left_shoulder_yaw_link">
+    <inertial>
+      <origin
+        xyz="0.01365 0.002767 -0.16266"
+        rpy="0 0 0" />
+      <mass
+        value="0.839" />
+      <inertia
+        ixx="0.003664"
+        ixy="-1.0671E-05"
+        ixz="0.00034733"
+        iyy="0.0040789"
+        iyz="7.0213E-05"
+        izz="0.00066383" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_shoulder_yaw_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_shoulder_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.1343"
+      rpy="0 0 0" />
+    <parent
+      link="left_shoulder_roll_link" />
+    <child
+      link="left_shoulder_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.3"
+      upper="4.45"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="left_elbow_link">
+    <inertial>
+      <origin
+        xyz="0.164862 0.000118 -0.015734"
+        rpy="0 0 0" />
+      <mass
+        value="0.723" />
+      <inertia
+        ixx="0.00049639"
+        ixy="-0.000033971"
+        ixz="0.000316026"
+        iyy="0.006456355"
+        iyz="0.000004929"
+        izz="0.006450997" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_elbow_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_elbow_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_elbow_joint"
+    type="revolute">
+    <origin
+      xyz="0.0185 0 -0.198"
+      rpy="0 0 0" />
+    <parent
+      link="left_shoulder_yaw_link" />
+    <child
+      link="left_elbow_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.25"
+      upper="2.61"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="left_hand_link">
+    <inertial>
+      <origin
+        xyz="0.003993 -0.000094 0"
+        rpy="0 0 0" />
+      <mass
+        value="0.014" />
+      <inertia
+        ixx="0.000003449"
+        ixy="0.000000006"
+        ixz="0"
+        iyy="0.000001917"
+        iyz="0"
+        izz="0.000001932" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hand_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/left_hand_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="left_hand_joint"
+    type="revolute">
+    <origin
+      xyz="0.2605 0 -0.0185"
+      rpy="0 0 0" />
+    <parent
+      link="left_elbow_link" />
+    <child
+      link="left_hand_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-3.0543261909900767596164588448551"
+      upper="3.0543261909900767596164588448551"
+      effort="6"
+      velocity="12" />
+  </joint>
+    <joint 
+      name="L_base_link_joint" 
+      type="fixed">
+      <origin
+      xyz="0.003 0 0"
+      rpy="0 0 1.5707963267948966192313216916398" />
+      <parent link="left_hand_link"/>
+      <child link="L_hand_base_link"/>
+    </joint>
+
+    <link name="L_hand_base_link">
+      <inertial>
+        <origin
+          xyz="-0.002551 -0.066047 -0.0019357"
+          rpy="0 0 0" />
+        <mass
+          value="0.14143" />
+        <inertia
+          ixx="0.0001234"
+          ixy="2.1995E-06"
+          ixz="-1.7694E-06"
+          iyy="8.3835E-05"
+          iyz="1.5968E-06"
+          izz="7.7231E-05" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/L_hand_base_link.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/L_hand_base_link.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="L_thumb_proximal_base">
+      <inertial>
+        <origin
+          xyz="0.0048817 0.00038782 -0.00722"
+          rpy="0 0 0" />
+        <mass
+          value="0.0018869" />
+        <inertia
+          ixx="5.5158E-08"
+          ixy="-1.1803E-08"
+          ixz="-4.6743E-09"
+          iyy="8.2164E-08"
+          iyz="-1.3521E-09"
+          izz="6.7434E-08" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link11_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="0.82745 0.83922 0.82745 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link11_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_thumb_proximal_yaw_joint"
+      type="revolute">
+      <origin
+        xyz="-0.01696 -0.0691 0.02045"
+        rpy="1.5708 -1.5708 0" />
+      <parent
+        link="L_hand_base_link" />
+      <child
+        link="L_thumb_proximal_base" />
+      <axis
+        xyz="0 0 1" />
+      <limit
+        lower="-0.1"
+        upper="1.3"
+        effort="1"
+        velocity="0.5" />
+    </joint>
+
+    <link name="L_thumb_proximal">
+      <inertial>
+        <origin
+          xyz="0.021936 -0.01279 -0.0080386"
+          rpy="0 0 0" />
+        <mass
+          value="0.0066101" />
+        <inertia
+          ixx="1.5693E-06"
+          ixy="7.8339E-07"
+          ixz="8.5959E-10"
+          iyy="1.7356E-06"
+          iyz="1.0378E-09"
+          izz="2.787E-06" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link12_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link12_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_thumb_proximal_pitch_joint"
+      type="revolute">
+      <origin
+        xyz="0.0099867 0.0098242 -0.0089"
+        rpy="-1.5708 0 0.16939" />
+      <parent
+        link="L_thumb_proximal_base" />
+      <child
+        link="L_thumb_proximal" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="-0.1"
+        upper="0.6"
+        effort="1"
+        velocity="0.5" />
+    </joint>
+
+    <link name="L_thumb_intermediate">
+      <inertial>
+        <origin
+          xyz="0.0095531 0.0016282 -0.0072002"
+          rpy="0 0 0" />
+        <mass
+          value="0.0037844" />
+        <inertia
+          ixx="3.6981E-07"
+          ixy="9.8603E-08"
+          ixz="-2.8173E-12"
+          iyy="3.2395E-07"
+          iyz="-2.8028E-12"
+          izz="4.6532E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link13_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link13_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_thumb_intermediate_joint"
+      type="revolute">
+      <origin
+        xyz="0.04407 -0.034553 -0.0008"
+        rpy="0 0 0" />
+      <parent
+        link="L_thumb_proximal" />
+      <child
+        link="L_thumb_intermediate" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="0.8"
+        effort="1"
+        velocity="0.5" />
+      <mimic 
+        joint="L_thumb_proximal_pitch_joint"
+        multiplier="1.6"
+        offset="0" />
+    </joint>
+
+    <link name="L_thumb_distal">
+      <inertial>
+        <origin
+          xyz="0.0092888 -0.004953 -0.0060033"
+          rpy="0 0 0" />
+        <mass
+          value="0.003344" />
+        <inertia
+          ixx="1.3632E-07"
+          ixy="5.6787E-08"
+          ixz="-9.1939E-11"
+          iyy="1.4052E-07"
+          iyz="1.2145E-10"
+          izz="2.0026E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link14_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link14_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_thumb_distal_joint"
+      type="revolute">
+      <origin
+        xyz="0.020248 -0.010156 -0.0012"
+        rpy="0 0 0" />
+      <parent
+        link="L_thumb_intermediate" />
+      <child
+        link="L_thumb_distal" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.2"
+        effort="1"
+        velocity="0.5" />
+      <mimic 
+        joint="L_thumb_proximal_pitch_joint"
+        multiplier="2.4"
+        offset="0" />
+    </joint>
+
+    <link name="L_index_proximal">
+      <inertial>
+        <origin
+          xyz="0.0012971 -0.011934 -0.0059998"
+          rpy="0 0 0" />
+        <mass
+          value="0.0042405" />
+        <inertia
+          ixx="6.6215E-07"
+          ixy="1.8442E-08"
+          ixz="1.3746E-12"
+          iyy="2.1167E-07"
+          iyz="-1.4773E-11"
+          izz="6.9402E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link15_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="0.79216 0.81961 0.93333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link15_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_index_proximal_joint"
+      type="revolute">
+      <origin
+        xyz="0.00028533 -0.13653 0.032268"
+        rpy="-0.034907 0 0" />
+      <parent
+        link="L_hand_base_link" />
+      <child
+        link="L_index_proximal" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+    </joint>
+
+    <link name="L_index_intermediate">
+      <inertial>
+        <origin
+          xyz="0.0021753 -0.019567 -0.005"
+          rpy="0 0 0" />
+        <mass
+          value="0.0045682" />
+        <inertia
+          ixx="7.6284E-07"
+          ixy="-8.063E-08"
+          ixz="3.6797E-13"
+          iyy="9.4308E-08"
+          iyz="1.5743E-13"
+          izz="7.8176E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link16_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link16_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_index_intermediate_joint"
+      type="revolute">
+      <origin
+        xyz="-0.0024229 -0.032041 -0.001"
+        rpy="0 0 0" />
+      <parent
+        link="L_index_proximal" />
+      <child
+        link="L_index_intermediate" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+      <mimic 
+        joint="L_index_proximal_joint"
+        multiplier="1"
+        offset="0" />
+    </joint>
+
+    <link name="L_middle_proximal">
+      <inertial>
+        <origin
+          xyz="0.0012971 -0.011934 -0.0059999"
+          rpy="0 0 0" />
+        <mass
+          value="0.0042405" />
+        <inertia
+          ixx="6.6215E-07"
+          ixy="1.8442E-08"
+          ixz="1.2299E-12"
+          iyy="2.1167E-07"
+          iyz="-1.4484E-11"
+          izz="6.9402E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link17_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="0.79216 0.81961 0.93333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link17_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_middle_proximal_joint"
+      type="revolute">
+      <origin
+        xyz="0.00028533 -0.1371 0.01295"
+        rpy="0 0 0" />
+      <parent
+        link="L_hand_base_link" />
+      <child
+        link="L_middle_proximal" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+    </joint>
+
+    <link name="L_middle_intermediate">
+      <inertial>
+        <origin
+          xyz="0.001921 -0.020796 -0.0049999"
+          rpy="0 0 0" />
+        <mass
+          value="0.0050397" />
+        <inertia
+          ixx="9.5823E-07"
+          ixy="-1.1425E-07"
+          ixz="-2.4186E-12"
+          iyy="1.0646E-07"
+          iyz="3.6974E-12"
+          izz="9.8385E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link18_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="0.82745 0.83922 0.82745 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link18_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+    
+    <joint
+      name="L_middle_intermediate_joint"
+      type="revolute">
+      <origin
+        xyz="-0.0024229 -0.032041 -0.001"
+        rpy="0 0 0" />
+      <parent
+        link="L_middle_proximal" />
+      <child
+        link="L_middle_intermediate" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+      <mimic 
+        joint="L_middle_proximal_joint"
+        multiplier="1"
+        offset="0" />
+    </joint>
+
+    <link name="L_ring_proximal">
+      <inertial>
+        <origin
+          xyz="0.0012971 -0.011934 -0.0059999"
+          rpy="0 0 0" />
+        <mass
+          value="0.0042405" />
+        <inertia
+          ixx="6.6215E-07"
+          ixy="1.8442E-08"
+          ixz="9.6052E-13"
+          iyy="2.1167E-07"
+          iyz="-1.4124E-11"
+          izz="6.9402E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link19_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="0.79216 0.81961 0.93333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link19_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_ring_proximal_joint"
+      type="revolute">
+      <origin
+        xyz="0.00028533 -0.13691 -0.0062872"
+        rpy="0.05236 0 0" />
+      <parent
+        link="L_hand_base_link" />
+      <child
+        link="L_ring_proximal" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+    </joint>
+
+    <link name="L_ring_intermediate">
+      <inertial>
+        <origin
+          xyz="0.0021753 -0.019567 -0.005"
+          rpy="0 0 0" />
+        <mass
+          value="0.0045682" />
+        <inertia
+          ixx="7.6285E-07"
+          ixy="-8.0631E-08"
+          ixz="3.3472E-14"
+          iyy="9.4308E-08"
+          iyz="-4.4773E-13"
+          izz="7.8176E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link20_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link20_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_ring_intermediate_joint"
+      type="revolute">
+      <origin
+        xyz="-0.0024229 -0.032041 -0.001"
+        rpy="0 0 0" />
+      <parent
+        link="L_ring_proximal" />
+      <child
+        link="L_ring_intermediate" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+      <mimic 
+        joint="L_ring_proximal_joint"
+        multiplier="1"
+        offset="0" />
+    </joint>
+
+    <link name="L_pinky_proximal">
+      <inertial>
+        <origin
+          xyz="0.0012971 -0.011934 -0.0059999"
+          rpy="0 0 0" />
+        <mass
+          value="0.0042405" />
+        <inertia
+          ixx="6.6215E-07"
+          ixy="1.8442E-08"
+          ixz="1.0279E-12"
+          iyy="2.1167E-07"
+          iyz="-1.4277E-11"
+          izz="6.9402E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link21_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="0.79216 0.81961 0.93333 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link21_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_pinky_proximal_joint"
+      type="revolute">
+      <origin
+        xyz="0.00028533 -0.13571 -0.025488"
+        rpy="0.10472 0 0" />
+      <parent
+        link="L_hand_base_link" />
+      <child
+        link="L_pinky_proximal" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+    </joint>
+
+    <link name="L_pinky_intermediate">
+      <inertial>
+        <origin
+          xyz="0.0024788 -0.016208 -0.0050001"
+          rpy="0 0 0" />
+        <mass
+          value="0.0036036" />
+        <inertia
+          ixx="4.3923E-07"
+          ixy="-4.1355E-08"
+          ixz="1.2263E-12"
+          iyy="7.0315E-08"
+          iyz="3.1311E-12"
+          izz="4.4881E-07" />
+      </inertial>
+      <visual>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link22_L.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1" />
+        </material>
+      </visual>
+      <collision>
+        <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://h1_description/meshes/Link22_L.STL" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint
+      name="L_pinky_intermediate_joint"
+      type="revolute">
+      <origin
+        xyz="-0.0024229 -0.032041 -0.001"
+        rpy="0 0 0" />
+      <parent
+        link="L_pinky_proximal" />
+      <child
+        link="L_pinky_intermediate" />
+      <axis
+        xyz="0 0 -1" />
+      <limit
+        lower="0"
+        upper="1.7"
+        effort="1"
+        velocity="0.5" />
+      <mimic 
+        joint="L_pinky_proximal_joint"
+        multiplier="1"
+        offset="0" />
+    </joint>
+  <link
+    name="right_shoulder_pitch_link">
+    <inertial>
+      <origin
+        xyz="0.005045 -0.053657 -0.015715"
+        rpy="0 0 0" />
+      <mass
+        value="1.033" />
+      <inertia
+        ixx="0.0012985"
+        ixy="1.7333E-05"
+        ixz="8.683E-06"
+        iyy="0.00087279"
+        iyz="-3.9656E-05"
+        izz="0.00097338" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_pitch_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_pitch_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_shoulder_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="0.0055 -0.15535 0.42999"
+      rpy="-0.43633 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="right_shoulder_pitch_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.87"
+      upper="2.87"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="right_shoulder_roll_link">
+    <inertial>
+      <origin
+        xyz="0.000679 -0.00115 -0.094076"
+        rpy="0 0 0" />
+      <mass
+        value="0.793" />
+      <inertia
+        ixx="0.0015742"
+        ixy="-2.298E-06"
+        ixz="-7.2265E-05"
+        iyy="0.0016973"
+        iyz="6.3691E-05"
+        izz="0.0010183" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_roll_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_roll_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_shoulder_roll_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0055 -0.0565 -0.0165"
+      rpy="0.43633 0 0" />
+    <parent
+      link="right_shoulder_pitch_link" />
+    <child
+      link="right_shoulder_roll_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-3.11"
+      upper="0.34"
+      effort="40"
+      velocity="9" />
+  </joint>
+  <link
+    name="right_shoulder_yaw_link">
+    <inertial>
+      <origin
+        xyz="0.01365 -0.002767 -0.16266"
+        rpy="0 0 0" />
+      <mass
+        value="0.839" />
+      <inertia
+        ixx="0.003664"
+        ixy="1.0671E-05"
+        ixz="0.00034733"
+        iyy="0.0040789"
+        iyz="-7.0213E-05"
+        izz="0.00066383" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_yaw_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_shoulder_yaw_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_shoulder_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 -0.1343"
+      rpy="0 0 0" />
+    <parent
+      link="right_shoulder_roll_link" />
+    <child
+      link="right_shoulder_yaw_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-4.45"
+      upper="1.3"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="right_elbow_link">
+    <inertial>
+      <origin
+        xyz="0.164862 -0.000118 -0.015734"
+        rpy="0 0 0" />
+      <mass
+        value="0.723" />
+      <inertia
+        ixx="0.00049639"
+        ixy="0.000033971"
+        ixz="0.000316026"
+        iyy="0.006456355"
+        iyz="-0.000004929"
+        izz="0.006450997" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_elbow_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_elbow_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_elbow_joint"
+    type="revolute">
+    <origin
+      xyz="0.0185 0 -0.198"
+      rpy="0 0 0" />
+    <parent
+      link="right_shoulder_yaw_link" />
+    <child
+      link="right_elbow_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-1.25"
+      upper="2.61"
+      effort="18"
+      velocity="20" />
+  </joint>
+  <link
+    name="right_hand_link">
+    <inertial>
+      <origin
+        xyz="0.003993 0.000094 0"
+        rpy="0 0 0" />
+      <mass
+        value="0.014" />
+      <inertia
+        ixx="0.000003449"
+        ixy="-0.000000006"
+        ixz="0"
+        iyy="0.000001917"
+        iyz="0"
+        izz="0.000001932" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hand_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/right_hand_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="right_hand_joint"
+    type="revolute">
+    <origin
+      xyz="0.2605 0 -0.0185"
+      rpy="0 0 0" />
+    <parent
+      link="right_elbow_link" />
+    <child
+      link="right_hand_link" />
+    <axis
+      xyz="1 0 0" />
+    <limit
+      lower="-3.0543261909900767596164588448551"
+      upper="3.0543261909900767596164588448551"
+      effort="6"
+      velocity="12" />
+  </joint>
+
+    <joint 
+      name="R_base_link_joint" 
+      type="fixed">
+      <origin
+      xyz="0.003 0 0"
+      rpy="3.1415926535897932384626433832795 0 -1.5707963267948966192313216916398" />
+      <parent link="right_hand_link"/>
+      <child link="R_hand_base_link"/>
+    </joint>
+
+  <link name="R_hand_base_link">
+    <inertial>
+      <origin
+        xyz="-0.0025264 -0.066047 0.0019598"
+        rpy="0 0 0" />
+      <mass
+        value="0.14143" />
+      <inertia
+        ixx="0.00012281"
+        ixy="2.1711E-06"
+        ixz="1.7709E-06"
+        iyy="8.3832E-05"
+        iyz="-1.6551E-06"
+        izz="7.6663E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/R_hand_base_link.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/R_hand_base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="R_thumb_proximal_base">
+    <inertial>
+      <origin
+        xyz="-0.0048064 0.0009382 -0.00757"
+        rpy="0 0 0" />
+      <mass
+        value="0.0018869" />
+      <inertia
+        ixx="5.816E-08"
+        ixy="1.4539E-08"
+        ixz="4.491E-09"
+        iyy="7.9161E-08"
+        iyz="-1.8727E-09"
+        izz="6.7433E-08" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link11_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link11_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_thumb_proximal_yaw_joint"
+    type="revolute">
+    <origin
+      xyz="-0.01696 -0.0691 -0.02045"
+      rpy="1.5708 -1.5708 0" />
+    <parent
+      link="R_hand_base_link" />
+    <child
+      link="R_thumb_proximal_base" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-0.1"
+      upper="1.3"
+      effort="1"
+      velocity="0.5" />
+  </joint>
+
+  <link name="R_thumb_proximal">
+    <inertial>
+      <origin
+        xyz="0.021932 0.012785 -0.0080386"
+        rpy="0 0 0" />
+      <mass
+        value="0.0066075" />
+      <inertia
+        ixx="1.5686E-06"
+        ixy="-7.8296E-07"
+        ixz="8.9143E-10"
+        iyy="1.7353E-06"
+        iyz="-1.0191E-09"
+        izz="2.786E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link12_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link12_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_thumb_proximal_pitch_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0088099 0.010892 -0.00925"
+      rpy="1.5708 0 2.8587" />
+    <parent
+      link="R_thumb_proximal_base" />
+    <child
+      link="R_thumb_proximal" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-0.1"
+      upper="0.6"
+      effort="1"
+      velocity="0.5" />
+  </joint>
+
+  <link name="R_thumb_intermediate">
+    <inertial>
+      <origin
+        xyz="0.0095544 -0.0016282 -0.0071997"
+        rpy="0 0 0" />
+      <mass
+        value="0.0037847" />
+      <inertia
+        ixx="3.6981E-07"
+        ixy="-9.8581E-08"
+        ixz="-4.7469E-12"
+        iyy="3.2394E-07"
+        iyz="1.0939E-12"
+        izz="4.6531E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link13_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link13_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_thumb_intermediate_joint"
+    type="revolute">
+    <origin
+      xyz="0.04407 0.034553 -0.0008"
+      rpy="0 0 0" />
+    <parent
+      link="R_thumb_proximal" />
+    <child
+      link="R_thumb_intermediate" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="0.8"
+      effort="1"
+      velocity="0.5" />
+    <mimic 
+      joint="R_thumb_proximal_pitch_joint"
+      multiplier="1.6"
+      offset="0" />
+  </joint>
+
+  <link name="R_thumb_distal">
+    <inertial>
+      <origin
+        xyz="0.0092888 0.0049529 -0.0060033"
+        rpy="0 0 0" />
+      <mass
+        value="0.0033441" />
+      <inertia
+        ixx="1.3632E-07"
+        ixy="-5.6788E-08"
+        ixz="-9.2764E-11"
+        iyy="1.4052E-07"
+        iyz="-1.2283E-10"
+        izz="2.0026E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link14_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link14_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_thumb_distal_joint"
+    type="revolute">
+    <origin
+      xyz="0.020248 0.010156 -0.0012"
+      rpy="0 0 0" />
+    <parent
+      link="R_thumb_intermediate" />
+    <child
+      link="R_thumb_distal" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.2"
+      effort="1"
+      velocity="0.5" />
+    <mimic 
+      joint="R_thumb_proximal_pitch_joint"
+      multiplier="2.4"
+      offset="0" />
+  </joint>
+
+  <link name="R_index_proximal">
+    <inertial>
+      <origin
+        xyz="0.0012259 0.011942 -0.0060001"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042403" />
+      <inertia
+        ixx="6.6232E-07"
+        ixy="-1.5775E-08"
+        ixz="1.8515E-12"
+        iyy="2.1146E-07"
+        iyz="-5.0828E-12"
+        izz="6.9398E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link15_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link15_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_index_proximal_joint"
+    type="revolute">
+    <origin
+      xyz="0.00028533 -0.13653 -0.032268"
+      rpy="-3.1067 0 0" />
+    <parent
+      link="R_hand_base_link" />
+    <child
+      link="R_index_proximal" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+  </joint>
+
+  <link name="R_index_intermediate">
+    <inertial>
+      <origin
+        xyz="0.0019697 0.019589 -0.005"
+        rpy="0 0 0" />
+      <mass
+        value="0.0045683" />
+      <inertia
+        ixx="7.6111E-07"
+        ixy="8.7637E-08"
+        ixz="-3.7751E-13"
+        iyy="9.6076E-08"
+        iyz="9.9444E-13"
+        izz="7.8179E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link16_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link16_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_index_intermediate_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0026138 0.032026 -0.001"
+      rpy="0 0 0" />
+    <parent
+      link="R_index_proximal" />
+    <child
+      link="R_index_intermediate" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+    <mimic 
+      joint="R_index_proximal_joint"
+      multiplier="1"
+      offset="0" />
+  </joint>
+
+  <link name="R_middle_proximal">
+    <inertial>
+      <origin
+        xyz="0.001297 0.011934 -0.0060001"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042403" />
+      <inertia
+        ixx="6.6211E-07"
+        ixy="-1.8461E-08"
+        ixz="1.8002E-12"
+        iyy="2.1167E-07"
+        iyz="-6.6808E-12"
+        izz="6.9397E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link17_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link17_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_middle_proximal_joint"
+    type="revolute">
+    <origin
+      xyz="0.00028533 -0.1371 -0.01295"
+      rpy="-3.1416 0 0" />
+    <parent
+      link="R_hand_base_link" />
+    <child
+      link="R_middle_proximal" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+  </joint>
+
+  <link name="R_middle_intermediate">
+    <inertial>
+      <origin
+        xyz="0.001921 0.020796 -0.005"
+        rpy="0 0 0" />
+      <mass
+        value="0.0050396" />
+      <inertia
+        ixx="9.5822E-07"
+        ixy="1.1425E-07"
+        ixz="-2.4791E-12"
+        iyy="1.0646E-07"
+        iyz="5.9173E-12"
+        izz="9.8384E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link18_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link18_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_middle_intermediate_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0024229 0.032041 -0.001"
+      rpy="0 0 0" />
+    <parent
+      link="R_middle_proximal" />
+    <child
+      link="R_middle_intermediate" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+    <mimic 
+      joint="R_middle_proximal_joint"
+      multiplier="1"
+      offset="0" />
+  </joint>
+
+  <link name="R_ring_proximal">
+    <inertial>
+      <origin
+        xyz="0.001297 0.011934 -0.0060002"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042403" />
+      <inertia
+        ixx="6.6211E-07"
+        ixy="-1.8461E-08"
+        ixz="1.5793E-12"
+        iyy="2.1167E-07"
+        iyz="-6.6868E-12"
+        izz="6.9397E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link19_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link19_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_ring_proximal_joint"
+    type="revolute">
+    <origin
+      xyz="0.00028533 -0.13691 0.0062872"
+      rpy="3.0892 0 0" />
+    <parent
+      link="R_hand_base_link" />
+    <child
+      link="R_ring_proximal" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+  </joint>
+
+  <link name="R_ring_intermediate">
+    <inertial>
+      <origin
+        xyz="0.0021753 0.019567 -0.005"
+        rpy="0 0 0" />
+      <mass
+        value="0.0045683" />
+      <inertia
+        ixx="7.6286E-07"
+        ixy="8.0635E-08"
+        ixz="-6.1562E-13"
+        iyy="9.431E-08"
+        iyz="5.8619E-13"
+        izz="7.8177E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link20_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link20_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_ring_intermediate_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0024229 0.032041 -0.001"
+      rpy="0 0 0" />
+    <parent
+      link="R_ring_proximal" />
+    <child
+      link="R_ring_intermediate" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+    <mimic 
+      joint="R_ring_proximal_joint"
+      multiplier="1"
+      offset="0" />
+  </joint>
+
+  <link name="R_pinky_proximal">
+    <inertial>
+      <origin
+        xyz="0.001297 0.011934 -0.0060001"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042403" />
+      <inertia
+        ixx="6.6211E-07"
+        ixy="-1.8461E-08"
+        ixz="1.6907E-12"
+        iyy="2.1167E-07"
+        iyz="-6.9334E-12"
+        izz="6.9397E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link21_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link21_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_pinky_proximal_joint"
+    type="revolute">
+    <origin
+      xyz="0.00028533 -0.13571 0.025488"
+      rpy="3.0369 0 0" />
+    <parent
+      link="R_hand_base_link" />
+    <child
+      link="R_pinky_proximal" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+  </joint>
+
+  <link name="R_pinky_intermediate">
+    <inertial>
+      <origin
+        xyz="0.0024748 0.016203 -0.0050031"
+        rpy="0 0 0" />
+      <mass
+        value="0.0035996" />
+      <inertia
+        ixx="4.3913E-07"
+        ixy="4.1418E-08"
+        ixz="3.7168E-11"
+        iyy="7.0247E-08"
+        iyz="5.8613E-11"
+        izz="4.4867E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link22_R.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://h1_description/meshes/Link22_R.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="R_pinky_intermediate_joint"
+    type="revolute">
+    <origin
+      xyz="-0.0024229 0.032041 -0.001"
+      rpy="0 0 0" />
+    <parent
+      link="R_pinky_proximal" />
+    <child
+      link="R_pinky_intermediate" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="1.7"
+      effort="1"
+      velocity="0.5" />
+    <mimic 
+      joint="R_pinky_proximal_joint"
+      multiplier="1"
+      offset="0" />
+  </joint>
+  <link
+    name="logo_link">
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/logo_link_gray.dae" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://h1_description/meshes/logo_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="logo_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="torso_link" />
+    <child
+      link="logo_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>
\ No newline at end of file