h1(4 DoF arm) IK test.

assets/h1_2/README.md

@@ -1,79 +0,0 @@
-# Unitree H1 Description (URDF & MJCF)
-
-## Overview
-
-This package includes a streamlined robot description (URDF & MJCF) for the [Unitree H1](https://www.unitree.com/h1/), developed by [Unitree Robotics](https://www.unitree.com/).
-
-<p align="center">
-  <img src="h1_2.png" width="500"/>
-</p>
-
-Unitree H1 have 51 DOFs:
-
-```text
-root [⚓] => /pelvis/
-    left_hip_yaw_joint [⚙+Z] => /left_hip_yaw_link/
-        left_hip_pitch_joint [⚙+Y] => /left_hip_pitch_link/
-            left_hip_roll_joint [⚙+X] => /left_hip_roll_link/
-                left_knee_joint [⚙+Y] => /left_knee_link/
-                    left_ankle_pitch_joint [⚙+Y] => /left_ankle_pitch_link/
-                        left_ankle_roll_joint [⚙+X] => /left_ankle_roll_link/
-    right_hip_yaw_joint [⚙+Z] => /right_hip_yaw_link/
-        right_hip_pitch_joint [⚙+Y] => /right_hip_pitch_link/
-            right_hip_roll_joint [⚙+X] => /right_hip_roll_link/
-                right_knee_joint [⚙+Y] => /right_knee_link/
-                    right_ankle_pitch_joint [⚙+Y] => /right_ankle_pitch_link/
-                        right_ankle_roll_joint [⚙+X] => /right_ankle_roll_link/
-    torso_joint [⚙+Z] => /torso_link/
-        left_shoulder_pitch_joint [⚙+Y] => /left_shoulder_pitch_link/
-            left_shoulder_roll_joint [⚙+X] => /left_shoulder_roll_link/
-                left_shoulder_yaw_joint [⚙+Z] => /left_shoulder_yaw_link/
-                    left_elbow_pitch_joint [⚙+Y] => /left_elbow_pitch_link/
-                        left_elbow_roll_joint [⚙+X] => /left_elbow_roll_link/
-                            left_wrist_pitch_joint [⚙+Y] => /left_wrist_pitch_link/
-                                left_wrist_yaw_joint [⚙+Z] => /left_wrist_yaw_link/
-                                    L_base_link_joint [⚓] => /L_hand_base_link/
-                                        L_thumb_proximal_yaw_joint [⚙+Z] => /L_thumb_proximal_base/
-                                            L_thumb_proximal_pitch_joint [⚙-Z] => /L_thumb_proximal/
-                                                L_thumb_intermediate_joint [⚙-Z] => /L_thumb_intermediate/
-                                                    L_thumb_distal_joint [⚙-Z] => /L_thumb_distal/
-                                        L_index_proximal_joint [⚙-Z] => /L_index_proximal/
-                                            L_index_intermediate_joint [⚙-Z] => /L_index_intermediate/
-                                        L_middle_proximal_joint [⚙-Z] => /L_middle_proximal/
-                                            L_middle_intermediate_joint [⚙-Z] => /L_middle_intermediate/
-                                        L_ring_proximal_joint [⚙-Z] => /L_ring_proximal/
-                                            L_ring_intermediate_joint [⚙-Z] => /L_ring_intermediate/
-                                        L_pinky_proximal_joint [⚙-Z] => /L_pinky_proximal/
-                                            L_pinky_intermediate_joint [⚙-Z] => /L_pinky_intermediate/
-        right_shoulder_pitch_joint [⚙+Y] => /right_shoulder_pitch_link/
-            right_shoulder_roll_joint [⚙+X] => /right_shoulder_roll_link/
-                right_shoulder_yaw_joint [⚙+Z] => /right_shoulder_yaw_link/
-                    right_elbow_pitch_joint [⚙+Y] => /right_elbow_pitch_link/
-                        right_elbow_roll_joint [⚙+X] => /right_elbow_roll_link/
-                            right_wrist_pitch_joint [⚙+Y] => /right_wrist_pitch_link/
-                                right_wrist_yaw_joint [⚙+Z] => /right_wrist_yaw_link/
-                                    R_base_link_joint [⚓] => /R_hand_base_link/
-                                        R_thumb_proximal_yaw_joint [⚙-Z] => /R_thumb_proximal_base/
-                                            R_thumb_proximal_pitch_joint [⚙+Z] => /R_thumb_proximal/
-                                                R_thumb_intermediate_joint [⚙+Z] => /R_thumb_intermediate/
-                                                    R_thumb_distal_joint [⚙+Z] => /R_thumb_distal/
-                                        R_index_proximal_joint [⚙+Z] => /R_index_proximal/
-                                            R_index_intermediate_joint [⚙+Z] => /R_index_intermediate/
-                                        R_middle_proximal_joint [⚙+Z] => /R_middle_proximal/
-                                            R_middle_intermediate_joint [⚙+Z] => /R_middle_intermediate/
-                                        R_ring_proximal_joint [⚙+Z] => /R_ring_proximal/
-                                            R_ring_intermediate_joint [⚙+Z] => /R_ring_intermediate/
-                                        R_pinky_proximal_joint [⚙+Z] => /R_pinky_proximal/
-                                            R_pinky_intermediate_joint [⚙+Z] => /R_pinky_intermediate/
-```
-
-## Visulization with [MuJoCo](https://github.com/google-deepmind/mujoco)
-
-1. Open MuJoCo Viewer
-
-   ```bash
-   pip install mujoco
-   python -m mujoco.viewer
-   ```
-
-2. Drag and drop the MJCF model file (`scene.xml`) to the MuJoCo Viewer.

assets/h1_2/h1_2.xml

@@ -1,438 +0,0 @@
-<mujoco model="h1_2">
-  <compiler angle="radian" meshdir="meshes/" autolimits="true"/>
-  <statistic meansize="0.112107" extent="1.95557" center="0.0256948 1.86841e-05 -0.178443"/>
-  <asset>
-    <mesh name="pelvis" file="pelvis.STL"/>
-    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL"/>
-    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL"/>
-    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL"/>
-    <mesh name="left_knee_link" file="left_knee_link.STL"/>
-    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL"/>
-    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL"/>
-    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL"/>
-    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL"/>
-    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL"/>
-    <mesh name="right_knee_link" file="right_knee_link.STL"/>
-    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL"/>
-    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL"/>
-    <mesh name="torso_link" file="torso_link.STL"/>
-    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL"/>
-    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL"/>
-    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL"/>
-    <mesh name="left_elbow_pitch_link" file="left_elbow_pitch_link.STL"/>
-    <mesh name="left_elbow_roll_link" file="left_elbow_roll_link.STL"/>
-    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL"/>
-    <mesh name="wrist_yaw_link" file="wrist_yaw_link.STL"/>
-    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL"/>
-    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL"/>
-    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL"/>
-    <mesh name="right_elbow_pitch_link" file="right_elbow_pitch_link.STL"/>
-    <mesh name="right_elbow_roll_link" file="right_elbow_roll_link.STL"/>
-    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL"/>
-    <mesh name="logo_link" file="logo_link.STL"/>
-    <mesh name="L_hand_base_link" file="L_hand_base_link.STL"/>
-    <mesh name="link11_L" file="link11_L.STL"/>
-    <mesh name="link12_L" file="link12_L.STL"/>
-    <mesh name="link13_L" file="link13_L.STL"/>
-    <mesh name="link14_L" file="link14_L.STL"/>
-    <mesh name="link15_L" file="link15_L.STL"/>
-    <mesh name="link16_L" file="link16_L.STL"/>
-    <mesh name="link17_L" file="link17_L.STL"/>
-    <mesh name="link18_L" file="link18_L.STL"/>
-    <mesh name="link19_L" file="link19_L.STL"/>
-    <mesh name="link20_L" file="link20_L.STL"/>
-    <mesh name="link21_L" file="link21_L.STL"/>
-    <mesh name="link22_L" file="link22_L.STL"/>
-    <mesh name="R_hand_base_link" file="R_hand_base_link.STL"/>
-    <mesh name="link11_R" file="link11_R.STL"/>
-    <mesh name="link12_R" file="link12_R.STL"/>
-    <mesh name="link13_R" file="link13_R.STL"/>
-    <mesh name="link14_R" file="link14_R.STL"/>
-    <mesh name="link15_R" file="link15_R.STL"/>
-    <mesh name="link16_R" file="link16_R.STL"/>
-    <mesh name="link17_R" file="link17_R.STL"/>
-    <mesh name="link18_R" file="link18_R.STL"/>
-    <mesh name="link19_R" file="link19_R.STL"/>
-    <mesh name="link20_R" file="link20_R.STL"/>
-    <mesh name="link21_R" file="link21_R.STL"/>
-    <mesh name="link22_R" file="link22_R.STL"/>
-  </asset>
-  <worldbody>
-    <body name="pelvis" pos="0 0 1.03">
-      <inertial pos="-0.0004 3.7e-05 -0.046864" quat="0.497097 0.496809 -0.503132 0.502925" mass="5.983" diaginertia="0.0531565 0.0491678 0.00902583"/>
-      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false"/>
-      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="pelvis"/>
-      <geom size="0.05" rgba="0.1 0.1 0.1 1"/>
-      <body name="left_hip_yaw_link" pos="0 0.0875 -0.1632">
-        <inertial pos="0 -0.026197 0.006647" quat="0.704899 -0.0553755 0.0548434 0.705013" mass="2.829" diaginertia="0.00574303 0.00455361 0.00349461"/>
-        <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-0.43 0.43" actuatorfrcrange="-200 200"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_hip_yaw_link"/>
-        <geom size="0.01 0.01" pos="0.02 0 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-        <body name="left_hip_pitch_link" pos="0 0.0755 0">
-          <inertial pos="-0.00781 -0.004724 -6.3e-05" quat="0.701575 0.711394 0.0330266 0.0249149" mass="2.92" diaginertia="0.00560661 0.00445055 0.00385068"/>
-          <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 2.5" actuatorfrcrange="-200 200"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_hip_pitch_link"/>
-          <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-          <body name="left_hip_roll_link">
-            <inertial pos="0.004171 -0.008576 -0.194509" quat="0.634842 0.0146079 0.0074063 0.772469" mass="4.962" diaginertia="0.0480229 0.0462788 0.00887409"/>
-            <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.43 3.14" actuatorfrcrange="-200 200"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_hip_roll_link"/>
-            <geom size="0.02 0.005" pos="0 0.06 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-            <body name="left_knee_link" pos="0 0 -0.4">
-              <inertial pos="0.000179 0.000121 -0.168936" quat="0.416585 0.0104983 0.00514003 0.909021" mass="3.839" diaginertia="0.0391044 0.038959 0.00501125"/>
-              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.26 2.05" actuatorfrcrange="-300 300"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_knee_link"/>
-              <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-              <body name="left_ankle_pitch_link" pos="0 0 -0.4">
-                <inertial pos="-0.000294 0 -0.010794" quat="0.999984 0 -0.00574445 0" mass="0.102" diaginertia="2.39454e-05 2.1837e-05 1.34126e-05"/>
-                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.897334 0.523598" actuatorfrcrange="-60 60"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_ankle_pitch_link"/>
-                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_ankle_pitch_link"/>
-                <body name="left_ankle_roll_link" pos="0 0 -0.02">
-                  <inertial pos="0.029589 0 -0.015973" quat="0 0.725858 0 0.687845" mass="0.747" diaginertia="0.00359178 0.00343534 0.000640307"/>
-                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.261799 0.261799" actuatorfrcrange="-40 40"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_ankle_roll_link"/>
-                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_ankle_roll_link"/>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-      <body name="right_hip_yaw_link" pos="0 -0.0875 -0.1632">
-        <inertial pos="0 0.026197 0.006647" quat="0.705013 0.0548434 -0.0553755 0.704899" mass="2.829" diaginertia="0.00574303 0.00455361 0.00349461"/>
-        <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-0.43 0.43" actuatorfrcrange="-200 200"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_hip_yaw_link"/>
-        <geom size="0.01 0.01" pos="0.02 0 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-        <body name="right_hip_pitch_link" pos="0 -0.0755 0">
-          <inertial pos="-0.00781 0.004724 -6.3e-05" quat="0.711394 0.701575 -0.0249149 -0.0330266" mass="2.92" diaginertia="0.00560661 0.00445055 0.00385068"/>
-          <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 2.5" actuatorfrcrange="-200 200"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_hip_pitch_link"/>
-          <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-          <body name="right_hip_roll_link">
-            <inertial pos="0.004171 0.008576 -0.194509" quat="0.772469 0.0074063 0.0146079 0.634842" mass="4.962" diaginertia="0.0480229 0.0462788 0.00887409"/>
-            <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-3.14 0.43" actuatorfrcrange="-200 200"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_hip_roll_link"/>
-            <geom size="0.02 0.005" pos="0 -0.06 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-            <body name="right_knee_link" pos="0 0 -0.4">
-              <inertial pos="0.000179 -0.000121 -0.168936" quat="0.909021 0.00514003 0.0104983 0.416585" mass="3.839" diaginertia="0.0391044 0.038959 0.00501125"/>
-              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.26 2.05" actuatorfrcrange="-300 300"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_knee_link"/>
-              <geom size="0.02 0.1" pos="0 0 -0.2" type="cylinder" rgba="0.1 0.1 0.1 1"/>
-              <body name="right_ankle_pitch_link" pos="0 0 -0.4">
-                <inertial pos="-0.000294 0 -0.010794" quat="0.999984 0 -0.00574445 0" mass="0.102" diaginertia="2.39454e-05 2.1837e-05 1.34126e-05"/>
-                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.897334 0.523598" actuatorfrcrange="-60 60"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_ankle_pitch_link"/>
-                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_ankle_pitch_link"/>
-                <body name="right_ankle_roll_link" pos="0 0 -0.02">
-                  <inertial pos="0.029589 0 -0.015973" quat="0 0.725858 0 0.687845" mass="0.747" diaginertia="0.00359178 0.00343534 0.000640307"/>
-                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.261799 0.261799" actuatorfrcrange="-40 40"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_ankle_roll_link"/>
-                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_ankle_roll_link"/>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-      <body name="torso_link">
-        <inertial pos="0.000489 0.002797 0.20484" quat="0.999989 -0.00130808 -0.00282289 -0.00349105" mass="17.789" diaginertia="0.487315 0.409628 0.127837"/>
-        <joint name="torso_joint" pos="0 0 0" axis="0 0 1" range="-2.35 2.35" actuatorfrcrange="-200 200"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="torso_link"/>
-        <geom size="0.04 0.08 0.05" pos="0 0 0.15" type="box" rgba="0.1 0.1 0.1 1"/>
-        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 1 1 1" mesh="logo_link"/>
-        <site name="imu" size="0.01" pos="-0.04452 -0.01891 0.27756"/>
-        <body name="left_shoulder_pitch_link" pos="0 0.14806 0.42333" quat="0.991445 0.130526 0 0">
-          <inertial pos="0.003053 0.06042 -0.0059" quat="0.761799 0.645681 -0.0378496 -0.0363943" mass="1.327" diaginertia="0.000588757 0.00053309 0.000393023"/>
-          <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 1.57" actuatorfrcrange="-40 40"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_pitch_link"/>
-          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_pitch_link"/>
-          <body name="left_shoulder_roll_link" pos="0.0342 0.061999 -0.0060011" quat="0.991445 -0.130526 0 0">
-            <inertial pos="-0.030932 -1e-06 -0.10609" quat="0.986055 0.000456937 0.166408 0.00213553" mass="1.393" diaginertia="0.00200869 0.00193464 0.000449847"/>
-            <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.38 3.4" actuatorfrcrange="-40 40"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_roll_link"/>
-            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_roll_link"/>
-            <body name="left_shoulder_yaw_link" pos="-0.0342 0 -0.1456">
-              <inertial pos="0.004583 0.001128 -0.001128" quat="0.663644 -0.0108866 -0.0267235 0.747492" mass="1.505" diaginertia="0.00431782 0.00420697 0.000645658"/>
-              <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.66 3.01" actuatorfrcrange="-18 18"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_yaw_link"/>
-              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_shoulder_yaw_link"/>
-              <body name="left_elbow_pitch_link" pos="0.006 0.0329 -0.182">
-                <inertial pos="0.077092 -0.028751 -0.009714" quat="0.544921 0.610781 0.423352 0.388305" mass="0.691" diaginertia="0.000942091 0.000905273 0.00023025"/>
-                <joint name="left_elbow_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.95 3.18" actuatorfrcrange="-18 18"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_elbow_pitch_link"/>
-                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_elbow_pitch_link"/>
-                <body name="left_elbow_roll_link" pos="0.121 -0.0329 -0.011">
-                  <inertial pos="0.035281 -0.00232 0.000337" quat="0.334998 0.622198 -0.240131 0.66557" mass="0.683" diaginertia="0.00034681 0.000328248 0.000294628"/>
-                  <joint name="left_elbow_roll_joint" pos="0 0 0" axis="1 0 0" range="-3.01 2.75" actuatorfrcrange="-19 19"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_elbow_roll_link"/>
-                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_elbow_roll_link"/>
-                  <body name="left_wrist_pitch_link" pos="0.085 0 0">
-                    <inertial pos="0.020395 3.6e-05 -0.002973" quat="0.915893 -0.228405 -0.327262 -0.0432527" mass="0.484" diaginertia="7.25675e-05 7.00325e-05 6.9381e-05"/>
-                    <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.47 0.47" actuatorfrcrange="-19 19"/>
-                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="left_wrist_pitch_link"/>
-                    <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="left_wrist_pitch_link"/>
-                    <body name="left_wrist_yaw_link" pos="0.02 0 0">
-                      <inertial pos="0.0770303 -0.00131441 -0.00068617" quat="0.499919 0.510625 0.506813 0.482165" mass="0.26543" diaginertia="0.000854397 0.000723298 0.00022115"/>
-                      <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.27 1.27" actuatorfrcrange="-19 19"/>
-                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="wrist_yaw_link"/>
-                      <geom pos="0.054 0 0" quat="0.707107 0 0 0.707107" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="L_hand_base_link"/>
-                      <geom pos="0.054 0 0" quat="0.707107 0 0 0.707107" type="mesh" rgba="0.1 0.1 0.1 1" mesh="L_hand_base_link"/>
-                      <body name="L_thumb_proximal_base" pos="0.1231 -0.01696 0.02045" quat="-2.59735e-06 0.707107 0 0.707107">
-                        <inertial pos="0.0048817 0.00038782 -0.00722" quat="0.445981 0.352284 0.495833 0.656617" mass="0.0018869" diaginertia="8.66031e-08 6.87331e-08 4.94199e-08"/>
-                        <joint name="L_thumb_proximal_yaw_joint" pos="0 0 0" axis="0 0 1" range="-0.1 1.3" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link11_L"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link11_L"/>
-                        <body name="L_thumb_proximal" pos="0.0099867 0.0098242 -0.0089" quat="0.704571 -0.704573 -0.0598169 0.0598167">
-                          <inertial pos="0.021936 -0.01279 -0.0080386" quat="0.25452 0.660687 -0.251949 0.659723" mass="0.0066101" diaginertia="2.78701e-06 2.44024e-06 8.6466e-07"/>
-                          <joint name="L_thumb_proximal_pitch_joint" pos="0 0 0" axis="0 0 -1" range="-0.1 0.6" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link12_L"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link12_L"/>
-                          <body name="L_thumb_intermediate" pos="0.04407 -0.034553 -0.0008">
-                            <inertial pos="0.0095531 0.0016282 -0.0072002" quat="0.30738 0.636732 -0.307526 0.636803" mass="0.0037844" diaginertia="4.6532e-07 4.48114e-07 2.45646e-07"/>
-                            <joint name="L_thumb_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 0.8" actuatorfrcrange="-1 1"/>
-                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link13_L"/>
-                            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link13_L"/>
-                            <body name="L_thumb_distal" pos="0.020248 -0.010156 -0.0012">
-                              <inertial pos="0.0092888 -0.004953 -0.0060033" quat="0.266264 0.65596 -0.262836 0.655544" mass="0.003344" diaginertia="2.0026e-07 1.95246e-07 8.1594e-08"/>
-                              <joint name="L_thumb_distal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.2" actuatorfrcrange="-1 1"/>
-                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link14_L"/>
-                              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link14_L"/>
-                            </body>
-                          </body>
-                        </body>
-                      </body>
-                      <body name="L_index_proximal" pos="0.19053 0.00028533 0.032268" quat="0.706999 -0.0123409 -0.0123409 0.706999">
-                        <inertial pos="0.0012971 -0.011934 -0.0059998" quat="0.489677 0.510115 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
-                        <joint name="L_index_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link15_L"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link15_L"/>
-                        <body name="L_index_intermediate" pos="-0.0024229 -0.032041 -0.001">
-                          <inertial pos="0.0021753 -0.019567 -0.005" quat="0.528694 0.469555 -0.528694 0.469555" mass="0.0045682" diaginertia="7.8176e-07 7.72427e-07 8.47209e-08"/>
-                          <joint name="L_index_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link16_L"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link16_L"/>
-                        </body>
-                      </body>
-                      <body name="L_middle_proximal" pos="0.1911 0.00028533 0.01295" quat="0.707107 0 0 0.707107">
-                        <inertial pos="0.0012971 -0.011934 -0.0059999" quat="0.489677 0.510115 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
-                        <joint name="L_middle_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link17_L"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link17_L"/>
-                        <body name="L_middle_intermediate" pos="-0.0024229 -0.032041 -0.001">
-                          <inertial pos="0.001921 -0.020796 -0.0049999" quat="0.531603 0.466115 -0.531728 0.466262" mass="0.0050397" diaginertia="9.8385e-07 9.73288e-07 9.14016e-08"/>
-                          <joint name="L_middle_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link18_L"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link18_L"/>
-                        </body>
-                      </body>
-                      <body name="L_ring_proximal" pos="0.19091 0.00028533 -0.0062872" quat="0.706864 0.0185099 0.0185099 0.706864">
-                        <inertial pos="0.0012971 -0.011934 -0.0059999" quat="0.489677 0.510114 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
-                        <joint name="L_ring_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link19_L"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link19_L"/>
-                        <body name="L_ring_intermediate" pos="-0.0024229 -0.032041 -0.001">
-                          <inertial pos="0.0021753 -0.019567 -0.005" quat="0.528694 0.469556 -0.528694 0.469556" mass="0.0045682" diaginertia="7.8176e-07 7.72437e-07 8.47208e-08"/>
-                          <joint name="L_ring_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link20_L"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link20_L"/>
-                        </body>
-                      </body>
-                      <body name="L_pinky_proximal" pos="0.18971 0.00028533 -0.025488" quat="0.706138 0.0370072 0.0370072 0.706138">
-                        <inertial pos="0.0012971 -0.011934 -0.0059999" quat="0.489677 0.510114 -0.489692 0.510099" mass="0.0042405" diaginertia="6.9402e-07 6.62904e-07 2.10916e-07"/>
-                        <joint name="L_pinky_proximal_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link21_L"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link21_L"/>
-                        <body name="L_pinky_intermediate" pos="-0.0024229 -0.032041 -0.001">
-                          <inertial pos="0.0024788 -0.016208 -0.0050001" quat="0.526797 0.471683 -0.526793 0.471687" mass="0.0036036" diaginertia="4.4881e-07 4.43809e-07 6.5736e-08"/>
-                          <joint name="L_pinky_intermediate_joint" pos="0 0 0" axis="0 0 -1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link22_L"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link22_L"/>
-                        </body>
-                      </body>
-                    </body>
-                  </body>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-        <body name="right_shoulder_pitch_link" pos="0 -0.14806 0.42333" quat="0.991445 -0.130526 0 0">
-          <inertial pos="0.003053 -0.06042 -0.0059" quat="0.645681 0.761799 0.0363943 0.0378496" mass="1.327" diaginertia="0.000588757 0.00053309 0.000393023"/>
-          <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.14 1.57" actuatorfrcrange="-40 40"/>
-          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_pitch_link"/>
-          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_pitch_link"/>
-          <body name="right_shoulder_roll_link" pos="0.0342 -0.061999 -0.0060011" quat="0.991445 0.130526 0 0">
-            <inertial pos="-0.030932 1e-06 -0.10609" quat="0.986055 -0.000456937 0.166408 -0.00213553" mass="1.393" diaginertia="0.00200869 0.00193464 0.000449847"/>
-            <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-3.4 0.38" actuatorfrcrange="-40 40"/>
-            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_roll_link"/>
-            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_roll_link"/>
-            <body name="right_shoulder_yaw_link" pos="-0.0342 0 -0.1456">
-              <inertial pos="0.004583 -0.001128 -0.001128" quat="0.747492 -0.0267235 -0.0108866 0.663644" mass="1.505" diaginertia="0.00431782 0.00420697 0.000645658"/>
-              <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-3.01 2.66" actuatorfrcrange="-18 18"/>
-              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_yaw_link"/>
-              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_shoulder_yaw_link"/>
-              <body name="right_elbow_pitch_link" pos="0.006 -0.0329 -0.182">
-                <inertial pos="0.077092 0.028751 -0.009714" quat="0.388305 0.423352 0.610781 0.544921" mass="0.691" diaginertia="0.000942091 0.000905273 0.00023025"/>
-                <joint name="right_elbow_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.95 3.18" actuatorfrcrange="-18 18"/>
-                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_elbow_pitch_link"/>
-                <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_elbow_pitch_link"/>
-                <body name="right_elbow_roll_link" pos="0.121 0.0329 -0.011">
-                  <inertial pos="0.035281 -0.00232 0.000337" quat="0.334998 0.622198 -0.240131 0.66557" mass="0.683" diaginertia="0.00034681 0.000328248 0.000294628"/>
-                  <joint name="right_elbow_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.75 3.01" actuatorfrcrange="-19 19"/>
-                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_elbow_roll_link"/>
-                  <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_elbow_roll_link"/>
-                  <body name="right_wrist_pitch_link" pos="0.085 0 0">
-                    <inertial pos="0.020395 3.6e-05 -0.002973" quat="0.915893 -0.228405 -0.327262 -0.0432527" mass="0.484" diaginertia="7.25675e-05 7.00325e-05 6.9381e-05"/>
-                    <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.47 0.47" actuatorfrcrange="-19 19"/>
-                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="right_wrist_pitch_link"/>
-                    <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="right_wrist_pitch_link"/>
-                    <body name="right_wrist_yaw_link" pos="0.02 0 0">
-                      <inertial pos="0.0770303 0.0013013 -0.000699011" quat="0.482149 0.506915 0.510629 0.499827" mass="0.26543" diaginertia="0.00085381 0.000722728 0.000221145"/>
-                      <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.27 1.27" actuatorfrcrange="-19 19"/>
-                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="wrist_yaw_link"/>
-                      <geom pos="0.054 0 0" quat="0 0.707107 -0.707107 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="R_hand_base_link"/>
-                      <geom pos="0.054 0 0" quat="0 0.707107 -0.707107 0" type="mesh" rgba="0.1 0.1 0.1 1" mesh="R_hand_base_link"/>
-                      <body name="R_thumb_proximal_base" pos="0.1231 0.01696 0.02045" quat="-0.707107 -2.59735e-06 -0.707107 0">
-                        <inertial pos="-0.0048064 0.0009382 -0.00757" quat="0.515015 0.680854 0.408023 0.323596" mass="0.0018869" diaginertia="8.66026e-08 6.8732e-08 4.94194e-08"/>
-                        <joint name="R_thumb_proximal_yaw_joint" pos="0 0 0" axis="0 0 -1" range="-0.1 1.3" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link11_R"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link11_R"/>
-                        <body name="R_thumb_proximal" pos="-0.0088099 0.010892 -0.00925" quat="0.0996843 0.0996847 0.700046 0.700044">
-                          <inertial pos="0.021932 0.012785 -0.0080386" quat="-0.254474 0.660716 0.251893 0.659733" mass="0.0066075" diaginertia="2.78601e-06 2.43933e-06 8.64566e-07"/>
-                          <joint name="R_thumb_proximal_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.1 0.6" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link12_R"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link12_R"/>
-                          <body name="R_thumb_intermediate" pos="0.04407 0.034553 -0.0008">
-                            <inertial pos="0.0095544 -0.0016282 -0.0071997" quat="0.636718 0.307389 -0.636802 0.307548" mass="0.0037847" diaginertia="4.6531e-07 4.48089e-07 2.45661e-07"/>
-                            <joint name="R_thumb_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 0.8" actuatorfrcrange="-1 1"/>
-                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link13_R"/>
-                            <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link13_R"/>
-                            <body name="R_thumb_distal" pos="0.020248 0.010156 -0.0012">
-                              <inertial pos="0.0092888 0.0049529 -0.0060033" quat="-0.266294 0.655967 0.262806 0.655537" mass="0.0033441" diaginertia="2.0026e-07 1.95247e-07 8.1593e-08"/>
-                              <joint name="R_thumb_distal_joint" pos="0 0 0" axis="0 0 1" range="0 1.2" actuatorfrcrange="-1 1"/>
-                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link14_R"/>
-                              <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link14_R"/>
-                            </body>
-                          </body>
-                        </body>
-                      </body>
-                      <body name="R_index_proximal" pos="0.19053 -0.00028533 0.032268" quat="0.706999 0.0123358 -0.0123358 -0.706999">
-                        <inertial pos="0.0012259 0.011942 -0.0060001" quat="0.50867 0.49121 -0.508643 0.491172" mass="0.0042403" diaginertia="6.9398e-07 6.62871e-07 2.10909e-07"/>
-                        <joint name="R_index_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link15_R"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link15_R"/>
-                        <body name="R_index_intermediate" pos="-0.0026138 0.032026 -0.001">
-                          <inertial pos="0.0019697 0.019589 -0.005" quat="0.466773 0.531152 -0.466773 0.531153" mass="0.0045683" diaginertia="7.8179e-07 7.72465e-07 8.47212e-08"/>
-                          <joint name="R_index_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link16_R"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link16_R"/>
-                        </body>
-                      </body>
-                      <body name="R_middle_proximal" pos="0.1911 -0.00028533 0.01295" quat="0.707107 -2.59735e-06 2.59735e-06 -0.707107">
-                        <inertial pos="0.001297 0.011934 -0.0060001" quat="0.510131 0.489693 -0.510105 0.489653" mass="0.0042403" diaginertia="6.9397e-07 6.62865e-07 2.10915e-07"/>
-                        <joint name="R_middle_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link17_R"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link17_R"/>
-                        <body name="R_middle_intermediate" pos="-0.0024229 0.032041 -0.001">
-                          <inertial pos="0.001921 0.020796 -0.005" quat="0.466148 0.531627 -0.466229 0.531705" mass="0.0050396" diaginertia="9.8384e-07 9.73279e-07 9.14014e-08"/>
-                          <joint name="R_middle_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link18_R"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link18_R"/>
-                        </body>
-                      </body>
-                      <body name="R_ring_proximal" pos="0.19091 -0.00028533 -0.0062872" quat="-0.706864 0.0185215 -0.0185215 0.706864">
-                        <inertial pos="0.001297 0.011934 -0.0060002" quat="0.510129 0.489691 -0.510107 0.489654" mass="0.0042403" diaginertia="6.9397e-07 6.62865e-07 2.10915e-07"/>
-                        <joint name="R_ring_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link19_R"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link19_R"/>
-                        <body name="R_ring_intermediate" pos="-0.0024229 0.032041 -0.001">
-                          <inertial pos="0.0021753 0.019567 -0.005" quat="0.469554 0.528695 -0.469554 0.528695" mass="0.0045683" diaginertia="7.8177e-07 7.72448e-07 8.4722e-08"/>
-                          <joint name="R_ring_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link20_R"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link20_R"/>
-                        </body>
-                      </body>
-                      <body name="R_pinky_proximal" pos="0.18971 -0.00028533 -0.025488" quat="-0.706138 0.0369975 -0.0369975 0.706138">
-                        <inertial pos="0.001297 0.011934 -0.0060001" quat="0.51013 0.489693 -0.510106 0.489653" mass="0.0042403" diaginertia="6.9397e-07 6.62865e-07 2.10915e-07"/>
-                        <joint name="R_pinky_proximal_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link21_R"/>
-                        <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link21_R"/>
-                        <body name="R_pinky_intermediate" pos="-0.0024229 0.032041 -0.001">
-                          <inertial pos="0.0024748 0.016203 -0.0050031" quat="0.47398 0.528862 -0.469291 0.524799" mass="0.0035996" diaginertia="4.4867e-07 4.43723e-07 6.56538e-08"/>
-                          <joint name="R_pinky_intermediate_joint" pos="0 0 0" axis="0 0 1" range="0 1.7" actuatorfrcrange="-1 1"/>
-                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.1 0.1 0.1 1" mesh="link22_R"/>
-                          <geom type="mesh" rgba="0.1 0.1 0.1 1" mesh="link22_R"/>
-                        </body>
-                      </body>
-                    </body>
-                  </body>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-
-
-  <actuator>
-    <motor name="left_hip_yaw_joint" joint="left_hip_yaw_joint"/>
-    <motor name="left_hip_pitch_joint" joint="left_hip_pitch_joint"/>
-    <motor name="left_hip_roll_joint" joint="left_hip_roll_joint"/>
-    <motor name="left_knee_joint" joint="left_knee_joint"/>
-    <motor name="left_ankle_pitch_joint" joint="left_ankle_pitch_joint"/>
-    <motor name="left_ankle_roll_joint" joint="left_ankle_roll_joint"/>
-    <motor name="right_hip_yaw_joint" joint="right_hip_yaw_joint"/>
-    <motor name="right_hip_pitch_joint" joint="right_hip_pitch_joint"/>
-    <motor name="right_hip_roll_joint" joint="right_hip_roll_joint"/>
-    <motor name="right_knee_joint" joint="right_knee_joint"/>
-    <motor name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint"/>
-    <motor name="right_ankle_roll_joint" joint="right_ankle_roll_joint"/>
-    <motor name="torso_joint" joint="torso_joint"/>
-    <motor name="left_shoulder_pitch_joint" joint="left_shoulder_pitch_joint"/>
-    <motor name="left_shoulder_roll_joint" joint="left_shoulder_roll_joint"/>
-    <motor name="left_shoulder_yaw_joint" joint="left_shoulder_yaw_joint"/>
-    <motor name="left_elbow_pitch_joint" joint="left_elbow_pitch_joint"/>
-    <motor name="left_elbow_roll_joint" joint="left_elbow_roll_joint"/>
-    <motor name="left_wrist_pitch_joint" joint="left_wrist_pitch_joint"/>
-    <motor name="left_wrist_yaw_joint" joint="left_wrist_yaw_joint"/>
-    <motor name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint"/>
-    <motor name="right_shoulder_roll_joint" joint="right_shoulder_roll_joint"/>
-    <motor name="right_shoulder_yaw_joint" joint="right_shoulder_yaw_joint"/>
-    <motor name="right_elbow_pitch_joint" joint="right_elbow_pitch_joint"/>
-    <motor name="right_elbow_roll_joint" joint="right_elbow_roll_joint"/>
-    <motor name="right_wrist_pitch_joint" joint="right_wrist_pitch_joint"/>
-    <motor name="right_wrist_yaw_joint" joint="right_wrist_yaw_joint"/>
-    <motor name="L_index_proximal_joint" joint="L_index_proximal_joint"/>
-    <motor name="L_index_intermediate_joint" joint="L_index_intermediate_joint"/>
-    <motor name="L_middle_proximal_joint" joint="L_middle_proximal_joint"/>
-    <motor name="L_middle_intermediate_joint" joint="L_middle_intermediate_joint"/>
-    <motor name="L_ring_proximal_joint" joint="L_ring_proximal_joint"/>
-    <motor name="L_ring_intermediate_joint" joint="L_ring_intermediate_joint"/>
-    <motor name="L_pinky_proximal_joint" joint="L_pinky_proximal_joint"/>
-    <motor name="L_pinky_intermediate_joint" joint="L_pinky_intermediate_joint"/>
-    <motor name="L_thumb_proximal_yaw_joint" joint="L_thumb_proximal_yaw_joint"/>
-    <motor name="L_thumb_proximal_pitch_joint" joint="L_thumb_proximal_pitch_joint"/>
-    <motor name="L_thumb_intermediate_joint" joint="L_thumb_intermediate_joint"/>
-    <motor name="L_thumb_distal_joint" joint="L_thumb_distal_joint"/>
-    <motor name="R_index_proximal_joint" joint="R_index_proximal_joint"/>
-    <motor name="R_index_intermediate_joint" joint="R_index_intermediate_joint"/>
-    <motor name="R_middle_proximal_joint" joint="R_middle_proximal_joint"/>
-    <motor name="R_middle_intermediate_joint" joint="R_middle_intermediate_joint"/>
-    <motor name="R_ring_proximal_joint" joint="R_ring_proximal_joint"/>
-    <motor name="R_ring_intermediate_joint" joint="R_ring_intermediate_joint"/>
-    <motor name="R_pinky_proximal_joint" joint="R_pinky_proximal_joint"/>
-    <motor name="R_pinky_intermediate_joint" joint="R_pinky_intermediate_joint"/>
-    <motor name="R_thumb_proximal_yaw_joint" joint="R_thumb_proximal_yaw_joint"/>
-    <motor name="R_thumb_proximal_pitch_joint" joint="R_thumb_proximal_pitch_joint"/>
-    <motor name="R_thumb_intermediate_joint" joint="R_thumb_intermediate_joint"/>
-    <motor name="R_thumb_distal_joint" joint="R_thumb_distal_joint"/>
-  </actuator>
-
-  <sensor>
-    <gyro name="imu-angular-velocity" site="imu" noise="5e-4" cutoff="34.9"/>
-    <accelerometer name="imu-linear-acceleration" site="imu" noise="1e-2" cutoff="157"/>
-  </sensor>
-</mujoco>

assets/h1_2/scene.xml

@@ -1,22 +0,0 @@
-<mujoco model="h1 scene">
-  <include file="h1_2.xml"/>
-
-  <statistic center="1.0 0.7 1.5" extent="0.8"/>
-
-  <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0.9 0.9 0.9"/>
-    <rgba haze="0.15 0.25 0.35 1"/>
-    <global azimuth="-140" elevation="-20"/>
-  </visual>
-
-  <asset>
-    <texture type="skybox" builtin="flat" rgb1="0 0 0" rgb2="0 0 0" width="512" height="3072"/>
-    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
-    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
-  </asset>
-
-  <worldbody>
-    <light pos="1 0 3.5" dir="0 0 -1" directional="true"/>
-    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
-  </worldbody>
-</mujoco>

assets/h1_description/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(h1_description)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir launch meshes urdf)
+  install(DIRECTORY ${dir}/
+          DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

assets/h1_description/README.md

@@ -0,0 +1,58 @@
+# Unitree H1 Description (URDF & MJCF)
+
+## Overview
+
+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 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>
+
+Basic H1 Humanoid have 19 joints:
+
+```text
+root [⚓] => /pelvis/
+    left_hip_yaw_joint [⚙+Z] => /left_hip_yaw_link/
+        left_hip_roll_joint [⚙+X] => /left_hip_roll_link/
+            left_hip_pitch_joint [⚙+Y] => /left_hip_pitch_link/
+                left_knee_joint [⚙+Y] => /left_knee_link/
+                    left_ankle_joint [⚙+Y] => /left_ankle_link/
+    right_hip_yaw_joint [⚙+Z] => /right_hip_yaw_link/
+        right_hip_roll_joint [⚙+X] => /right_hip_roll_link/
+            right_hip_pitch_joint [⚙+Y] => /right_hip_pitch_link/
+                right_knee_joint [⚙+Y] => /right_knee_link/
+                    right_ankle_joint [⚙+Y] => /right_ankle_link/
+    torso_joint [⚙+Z] => /torso_link/
+        left_shoulder_pitch_joint [⚙+Y] => /left_shoulder_pitch_link/
+            left_shoulder_roll_joint [⚙+X] => /left_shoulder_roll_link/
+                left_shoulder_yaw_joint [⚙+Z] => /left_shoulder_yaw_link/
+                    left_elbow_joint [⚙+Y] => /left_elbow_link/
+        right_shoulder_pitch_joint [⚙+Y] => /right_shoulder_pitch_link/
+            right_shoulder_roll_joint [⚙+X] => /right_shoulder_roll_link/
+                right_shoulder_yaw_joint [⚙+Z] => /right_shoulder_yaw_link/
+                    right_elbow_joint [⚙+Y] => /right_elbow_link/
+```
+
+## Usages
+
+### [MuJoCo](https://github.com/google-deepmind/mujoco)(recommend)
+
+```bash
+pip install mujoco
+python -m mujoco.viewer --mjcf=mjcf/scene.xml
+```
+
+### RViz
+
+```bash
+roslaunch h1_description display.launch
+```
+
+### Gazebo
+
+```bash
+roslaunch h1_description gazebo.launch
+```

assets/h1_description/launch/check_joint.rviz

@@ -0,0 +1,239 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 719
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        pelvis:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        hip_rotation_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hip_abduction_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hip_flexion_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        knee_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ankle_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        hip_rotation_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hip_abduction_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hip_flexion_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        knee_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ankle_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        pelvis:
+          Value: true
+        hip_rotation_link_left:
+          Value: true
+        hip_abduction_link_left:
+          Value: true
+        hip_flexion_link_left:
+          Value: true
+        knee_link_left:
+          Value: true
+        ankle_link_left:
+          Value: true
+        hip_rotation_link_right:
+          Value: true
+        hip_abduction_link_right:
+          Value: true
+        hip_flexion_link_right:
+          Value: true
+        knee_link_right:
+          Value: true
+        ankle_link_right:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        pelvis:
+          hip_rotation_link_left:
+            hip_abduction_link_left:
+              hip_flexion_link_left:
+                knee_link_left:
+                  ankle_link_left:
+                    {}
+          hip_rotation_link_right:
+            hip_abduction_link_right:
+              hip_flexion_link_right:
+                knee_link_right:
+                  ankle_link_right:
+                    {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: pelvis
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.6701574325561523
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.11145864427089691
+        Y: 0.10033124685287476
+        Z: -0.35909000039100647
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5303982496261597
+      Target Frame: <Fixed Frame>
+      Yaw: 0.8653979301452637
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074c0000003efc0100000002fb0000000800540069006d006501000000000000074c000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004db0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1868
+  X: 52
+  Y: 27

assets/h1_description/launch/display.launch

@@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find h1_description)/urdf/h1_with_hand.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find h1_description)/launch/check_joint.rviz" />
+</launch>

assets/h1_description/launch/gazebo.launch

@@ -0,0 +1,12 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="paused" value="true" />
+  </include>
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find h1_description)/urdf/h1.urdf -urdf -z 1.05 -model h1_description"
+    output="screen" />
+</launch>