[update] support h1_2 in g1 branch.

README.md

@@ -156,38 +156,15 @@ Copy the `rootCA.pem` via AirDrop to Apple Vision Pro and install it.
 
 Settings > General > About > Certificate Trust Settings. Under "Enable full trust for root certificates", turn on trust for the certificate.
 
+> In the new version of Vision OS 2, this step is different: After copying the certificate to the Apple Vision Pro device via AirDrop, a certificate-related information section will appear below the account bar in the top left corner of the Settings app. Tap it to enable trust for the certificate.
+
 Settings > Apps > Safari > Advanced > Feature Flags > Enable WebXR Related Features.
 
-## 2.3 🔎 Test environment
+## 2.3 🔎 Unit Test
 
 This step is to verify that the environment is installed correctly.
 
-1. Download Isaac Gym: https://developer.nvidia.com/isaac-gym/download
-
-    Extracting to the current directory, go to the `IsaacGym_Preview_4_Package/isaacgym/python` directory and execute the command:
-
-    ```bash
-    (tv) unitree@Host:~/IsaacGym_Preview_4_Package/isaacgym/python$ pip install -e .
-    ```
-
-2. After setup up streaming with local following the above instructions, you can try teleoperating two robot hands in Issac Gym:
-
-    ```bash
-    (tv) unitree@Host:~/avp_teleoperate$ cd teleop
-    (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_test_gym.py
-    ```
-
-3. Wear your Apple Vision Pro device.
-
-4. Open Safari on Apple Vision Pro and visit: https://192.168.123.2:8012?ws=wss://192.168.123.2:8012
-
-    > p.s. This IP address should match the IP address of your **Host machine**.
-
-5. Click `Enter VR` and `Allow` to start the VR session.
-
-6. See your hands in 3D!
-
-
+comming soon.
 
 
 

README_zh-CN.md

@@ -159,36 +159,13 @@ unitree@Host:~$ conda activate tv
 
 设置 > 应用 > Safari > 高级 > 功能标志 > 启用 WebXR 相关功能。
 
-## 2.3 🔎 测试环境
+> 提醒：在新版本 Vision OS 2 系统中，该步骤有所不同：将证书通过 AirDrop 复制到 Apple Vision Pro 设备后，将会在设置 APP 中左上角账户栏的下方出现证书相关信息栏，点击进去即可启用对该证书的信任。
 
-此步骤用于验证环境是否正确安装。
-
-1. 下载 Isaac Gym：https://developer.nvidia.com/isaac-gym/download
-
-   解压到当前目录，进入 `IsaacGym_Preview_4_Package/isaacgym/python` 目录，执行命令：
-
-   ```bash
-   (tv) unitree@Host:~/IsaacGym_Preview_4_Package/isaacgym/python$ pip install -e .
-   ```
-
-2. 按照上述说明设置本地流媒体后，您可以尝试在 Isaac Gym 中远程操作两个机器人手：
-
-   ```bash
-   (tv) unitree@Host:~/avp_teleoperate$ cd teleop
-   (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_test_gym.py
-   ```
-
-3. 戴上您的 Apple Vision Pro 设备。
-
-4. 在 Apple Vision Pro 上打开 Safari，访问：https://192.168.123.2:8012?ws=wss://192.168.123.2:8012
-
-   > 提醒：此 IP 地址应与您的 **主机** IP 地址匹配。
-
-5. 点击 `Enter VR` 并选择 `Allow` 以启动 VR 会话。
-
-6. 在 3D 中看到您的手！
+## 2.3 🔎 单元测试
 
+此步骤用于验证环境是否正确安装。
 
+即将展现。
 
 
 

assets/h1_2/README.md

@@ -0,0 +1,79 @@
+# 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

@@ -0,0 +1,438 @@
+<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

@@ -0,0 +1,22 @@
+<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>

teleop/image_server/image_server.py

@@ -102,7 +102,7 @@ class OpenCVCamera():
 class ImageServer:
     def __init__(self, config, port = 5555, Unit_Test = False):
         """
-        config:
+        config example1:
         {
             'fps':30                                                          # frame per second
             'head_camera_type': 'opencv',                                     # opencv or realsense
@@ -110,7 +110,30 @@ class ImageServer:
             'head_camera_id_numbers': [0],                                    # '/dev/video0' (opencv)
             'wrist_camera_type': 'realsense', 
             'wrist_camera_image_shape': [480, 640],                           # Wrist camera resolution  [height, width]
-            'wrist_camera_id_numbers': ["218622271789", "241222076627"],      # serial number (realsense)
+            'wrist_camera_id_numbers': ["218622271789", "241222076627"],      # realsense camera's serial number
+        }
+
+        config example2:
+        {
+            'fps':30                                                          # frame per second
+            'head_camera_type': 'realsense',                                  # opencv or realsense
+            'head_camera_image_shape': [480, 640],                            # Head camera resolution  [height, width]
+            'head_camera_id_numbers': ["218622271739"],                       # realsense camera's serial number
+            'wrist_camera_type': 'opencv', 
+            'wrist_camera_image_shape': [480, 640],                           # Wrist camera resolution  [height, width]
+            'wrist_camera_id_numbers': [0,1],                                 # '/dev/video0' and '/dev/video1' (opencv)
+        }
+
+        If you are not using the wrist camera, you can comment out its configuration, like this below:
+        config:
+        {
+            'fps':30                                                          # frame per second
+            'head_camera_type': 'opencv',                                     # opencv or realsense
+            'head_camera_image_shape': [480, 1280],                           # Head camera resolution  [height, width]
+            'head_camera_id_numbers': [0],                                    # '/dev/video0' (opencv)
+            #'wrist_camera_type': 'realsense', 
+            #'wrist_camera_image_shape': [480, 640],                           # Wrist camera resolution  [height, width]
+            #'wrist_camera_id_numbers': ["218622271789", "241222076627"],      # serial number (realsense)
         }
         """
         print(config)

teleop/robot_control/robot_arm.py

@@ -11,6 +11,7 @@ from unitree_sdk2py.utils.crc import CRC
 kTopicLowCommand = "rt/lowcmd"
 kTopicLowState = "rt/lowstate"
 G1_29_Num_Motors = 35
+H1_2_Num_Motors = 35
  
 
 class MotorState:
@@ -22,6 +23,9 @@ class G1_29_LowState:
     def __init__(self):
         self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)]
 
+class H1_2_LowState:
+    def __init__(self):
+        self.motor_state = [MotorState() for _ in range(H1_2_Num_Motors)]
 class DataBuffer:
     def __init__(self):
         self.data = None
@@ -299,12 +303,277 @@ class G1_29_JointIndex(IntEnum):
     kNotUsedJoint4 = 33
     kNotUsedJoint5 = 34
 
+class H1_2_ArmController:
+    def __init__(self):
+        print("Initialize H1_2_ArmController...")
+        self.q_target = np.zeros(14)
+        self.tauff_target = np.zeros(14)
+
+        self.kp_high = 200.0
+        self.kd_high = 5.0
+        self.kp_low = 140.0
+        self.kd_low = 7.5
+        self.kp_wrist = 40.0
+        self.kd_wrist = 6.0
+
+        self.all_motor_q = None
+        self.arm_velocity_limit = 20.0
+        self.control_dt = 1.0 / 250.0
+
+        self._speed_gradual_max = False
+        self._gradual_start_time = None
+        self._gradual_time = None
+
+        # initialize lowcmd publisher and lowstate subscriber
+        ChannelFactoryInitialize(0)
+        self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, LowCmd_)
+        self.lowcmd_publisher.Init()
+        self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, LowState_)
+        self.lowstate_subscriber.Init()
+        self.lowstate_buffer = DataBuffer()
+
+        # initialize subscribe thread
+        self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state)
+        self.subscribe_thread.daemon = True
+        self.subscribe_thread.start()
+
+        while not self.lowstate_buffer.GetData():
+            time.sleep(0.01)
+            print("[H1_2_ArmController] Waiting to subscribe dds...")
+
+        # initialize hg's lowcmd msg
+        self.crc = CRC()
+        self.msg = unitree_hg_msg_dds__LowCmd_()
+        self.msg.mode_pr = 0
+        self.msg.mode_machine = self.get_mode_machine()
+
+        self.all_motor_q = self.get_current_motor_q()
+        print(f"Current all body motor state q:\n{self.all_motor_q} \n")
+        print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n")
+        print("Lock all joints except two arms...\n")
+
+        arm_indices = set(member.value for member in H1_2_JointArmIndex)
+        for id in H1_2_JointIndex:
+            self.msg.motor_cmd[id].mode = 1
+            if id.value in arm_indices:
+                if self._Is_wrist_motor(id):
+                    self.msg.motor_cmd[id].kp = self.kp_wrist
+                    self.msg.motor_cmd[id].kd = self.kd_wrist
+                else:
+                    self.msg.motor_cmd[id].kp = self.kp_low
+                    self.msg.motor_cmd[id].kd = self.kd_low
+            else:
+                if self._Is_weak_motor(id):
+                    self.msg.motor_cmd[id].kp = self.kp_low
+                    self.msg.motor_cmd[id].kd = self.kd_low
+                else:
+                    self.msg.motor_cmd[id].kp = self.kp_high
+                    self.msg.motor_cmd[id].kd = self.kd_high
+            self.msg.motor_cmd[id].q  = self.all_motor_q[id]
+        print("Lock OK!\n")
+
+        # initialize publish thread
+        self.publish_thread = threading.Thread(target=self._ctrl_motor_state)
+        self.ctrl_lock = threading.Lock()
+        self.publish_thread.daemon = True
+        self.publish_thread.start()
+
+        print("Initialize H1_2_ArmController OK!\n")
+
+    def _subscribe_motor_state(self):
+        while True:
+            msg = self.lowstate_subscriber.Read()
+            if msg is not None:
+                lowstate = H1_2_LowState()
+                for id in range(H1_2_Num_Motors):
+                    lowstate.motor_state[id].q  = msg.motor_state[id].q
+                    lowstate.motor_state[id].dq = msg.motor_state[id].dq
+                self.lowstate_buffer.SetData(lowstate)
+            time.sleep(0.002)
+
+    def clip_arm_q_target(self, target_q, velocity_limit):
+        current_q = self.get_current_dual_arm_q()
+        delta = target_q - current_q
+        motion_scale = np.max(np.abs(delta)) / (velocity_limit * self.control_dt)
+        cliped_arm_q_target = current_q + delta / max(motion_scale, 1.0)
+        return cliped_arm_q_target
+
+    def _ctrl_motor_state(self):
+        while True:
+            start_time = time.time()
+
+            with self.ctrl_lock:
+                arm_q_target     = self.q_target
+                arm_tauff_target = self.tauff_target
+
+            cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit)
+
+            for idx, id in enumerate(G1_29_JointArmIndex):
+                self.msg.motor_cmd[id].q = cliped_arm_q_target[idx]
+                self.msg.motor_cmd[id].dq = 0
+                self.msg.motor_cmd[id].tau = arm_tauff_target[idx]      
+
+            self.msg.crc = self.crc.Crc(self.msg)
+            self.lowcmd_publisher.Write(self.msg)
+
+            if self._speed_gradual_max is True:
+                t_elapsed = start_time - self._gradual_start_time
+                self.arm_velocity_limit = 20.0 + (10.0 * min(1.0, t_elapsed / 5.0))
+
+            current_time = time.time()
+            all_t_elapsed = current_time - start_time
+            sleep_time = max(0, (self.control_dt - all_t_elapsed))
+            time.sleep(sleep_time)
+            # print(f"arm_velocity_limit:{self.arm_velocity_limit}")
+            # print(f"sleep_time:{sleep_time}")
+
+    def ctrl_dual_arm(self, q_target, tauff_target):
+        '''Set control target values q & tau of the left and right arm motors.'''
+        with self.ctrl_lock:
+            self.q_target = q_target
+            self.tauff_target = tauff_target
+
+    def get_mode_machine(self):
+        '''Return current dds mode machine.'''
+        return self.lowstate_subscriber.Read().mode_machine
+    
+    def get_current_motor_q(self):
+        '''Return current state q of all body motors.'''
+        return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in H1_2_JointIndex])
+    
+    def get_current_dual_arm_q(self):
+        '''Return current state q of the left and right arm motors.'''
+        return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in H1_2_JointArmIndex])
+    
+    def get_current_dual_arm_dq(self):
+        '''Return current state dq of the left and right arm motors.'''
+        return np.array([self.lowstate_buffer.GetData().motor_state[id].dq for id in H1_2_JointArmIndex])
+    
+    def ctrl_dual_arm_go_home(self):
+        '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.'''
+        print("[H1_2_ArmController] ctrl_dual_arm_go_home start...")
+        with self.ctrl_lock:
+            self.q_target = np.zeros(14)
+            # self.tauff_target = np.zeros(14)
+        tolerance = 0.05  # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements
+        while True:
+            current_q = self.get_current_dual_arm_q()
+            if np.all(np.abs(current_q) < tolerance):
+                print("[H1_2_ArmController] both arms have reached the home position.")
+                break
+            time.sleep(0.05)
+
+    def speed_gradual_max(self, t = 5.0):
+        '''Parameter t is the total time required for arms velocity to gradually increase to its maximum value, in seconds. The default is 5.0.'''
+        self._gradual_start_time = time.time()
+        self._gradual_time = t
+        self._speed_gradual_max = True
+
+    def speed_instant_max(self):
+        '''set arms velocity to the maximum value immediately, instead of gradually increasing.'''
+        self.arm_velocity_limit = 30.0
+
+    def _Is_weak_motor(self, motor_index):
+        weak_motors = [
+            H1_2_JointIndex.kLeftAnkle.value,
+            H1_2_JointIndex.kRightAnkle.value,
+            # Left arm
+            H1_2_JointIndex.kLeftShoulderPitch.value,
+            H1_2_JointIndex.kLeftShoulderRoll.value,
+            H1_2_JointIndex.kLeftShoulderYaw.value,
+            H1_2_JointIndex.kLeftElbowPitch.value,
+            # Right arm
+            H1_2_JointIndex.kRightShoulderPitch.value,
+            H1_2_JointIndex.kRightShoulderRoll.value,
+            H1_2_JointIndex.kRightShoulderYaw.value,
+            H1_2_JointIndex.kRightElbowPitch.value,
+        ]
+        return motor_index.value in weak_motors
+    
+    def _Is_wrist_motor(self, motor_index):
+        wrist_motors = [
+            H1_2_JointIndex.kLeftElbowRoll.value,
+            H1_2_JointIndex.kLeftWristPitch.value,
+            H1_2_JointIndex.kLeftWristyaw.value,
+            H1_2_JointIndex.kRightElbowRoll.value,
+            H1_2_JointIndex.kRightWristPitch.value,
+            H1_2_JointIndex.kRightWristYaw.value,
+        ]
+        return motor_index.value in wrist_motors
+    
+class H1_2_JointArmIndex(IntEnum):
+    # Left arm
+    kLeftShoulderPitch = 13
+    kLeftShoulderRoll = 14
+    kLeftShoulderYaw = 15
+    kLeftElbowPitch = 16
+    kLeftElbowRoll = 17
+    kLeftWristPitch = 18
+    kLeftWristyaw = 19
+
+    # Right arm
+    kRightShoulderPitch = 20
+    kRightShoulderRoll = 21
+    kRightShoulderYaw = 22
+    kRightElbowPitch = 23
+    kRightElbowRoll = 24
+    kRightWristPitch = 25
+    kRightWristYaw = 26
+
+class H1_2_JointIndex(IntEnum):
+    # Left leg
+    kLeftHipYaw = 0
+    kLeftHipRoll = 1
+    kLeftHipPitch = 2
+    kLeftKnee = 3
+    kLeftAnkle = 4
+    kLeftAnkleRoll = 5
+
+    # Right leg
+    kRightHipYaw = 6
+    kRightHipRoll = 7
+    kRightHipPitch = 8
+    kRightKnee = 9
+    kRightAnkle = 10
+    kRightAnkleRoll = 11
+
+    kWaistYaw = 12
+
+    # Left arm
+    kLeftShoulderPitch = 13
+    kLeftShoulderRoll = 14
+    kLeftShoulderYaw = 15
+    kLeftElbowPitch = 16
+    kLeftElbowRoll = 17
+    kLeftWristPitch = 18
+    kLeftWristyaw = 19
+
+    # Right arm
+    kRightShoulderPitch = 20
+    kRightShoulderRoll = 21
+    kRightShoulderYaw = 22
+    kRightElbowPitch = 23
+    kRightElbowRoll = 24
+    kRightWristPitch = 25
+    kRightWristYaw = 26
+
+    kNotUsedJoint0 = 27
+    kNotUsedJoint1 = 28
+    kNotUsedJoint2 = 29
+    kNotUsedJoint3 = 30
+    kNotUsedJoint4 = 31
+    kNotUsedJoint5 = 32
+    kNotUsedJoint6 = 33
+    kNotUsedJoint7 = 34
+
 if __name__ == "__main__":
-    from robot_arm_ik import G1_29_ArmIK
+    from robot_arm_ik import G1_29_ArmIK, H1_2_ArmIK
     import pinocchio as pin 
 
-    arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = False)
-    g1arm = G1_29_ArmController()
+    # arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = False)
+    # arm = G1_29_ArmController()
+    arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = False)
+    arm = H1_2_ArmController()
 
     # initial positon
     L_tf_target = pin.SE3(
@@ -324,7 +593,7 @@ if __name__ == "__main__":
     user_input = input("Please enter the start signal (enter 's' to start the subsequent program): \n")
     if user_input.lower() == 's':
         step = 0
-        g1arm.speed_gradual_max()
+        arm.speed_gradual_max()
         while True:
             if step <= 120:
                 angle = rotation_speed * step
@@ -344,12 +613,12 @@ if __name__ == "__main__":
             L_tf_target.rotation = L_quat.toRotationMatrix()
             R_tf_target.rotation = R_quat.toRotationMatrix()
 
-            current_lr_arm_q  = g1arm.get_current_dual_arm_q()
-            current_lr_arm_dq = g1arm.get_current_dual_arm_dq()
+            current_lr_arm_q  = arm.get_current_dual_arm_q()
+            current_lr_arm_dq = arm.get_current_dual_arm_dq()
 
             sol_q, sol_tauff = arm_ik.solve_ik(L_tf_target.homogeneous, R_tf_target.homogeneous, current_lr_arm_q, current_lr_arm_dq)
 
-            g1arm.ctrl_dual_arm(sol_q, sol_tauff)
+            arm.ctrl_dual_arm(sol_q, sol_tauff)
 
             step += 1
             if step > 240:

teleop/robot_control/robot_arm_ik.py

@@ -258,9 +258,260 @@ class G1_29_ArmIK:
             # return sol_q, sol_tauff
             return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv)
 
+class H1_2_ArmIK:
+    def __init__(self, Unit_Test = False, Visualization = False):
+        np.set_printoptions(precision=5, suppress=True, linewidth=200)
+
+        self.Unit_Test = Unit_Test
+        self.Visualization = Visualization
+
+        if not self.Unit_Test:
+            self.robot = pin.RobotWrapper.BuildFromURDF('../assets/h1_2/h1_2.urdf', '../assets/h1_2/')
+        else:
+            self.robot = pin.RobotWrapper.BuildFromURDF('../../assets/h1_2/h1_2.urdf', '../../assets/h1_2/') # for test
+
+        self.mixed_jointsToLockIDs = [
+                                      "left_hip_yaw_joint",
+                                      "left_hip_pitch_joint",
+                                      "left_hip_roll_joint",
+                                      "left_knee_joint",
+                                      "left_ankle_pitch_joint",
+                                      "left_ankle_roll_joint",
+                                      "right_hip_yaw_joint",
+                                      "right_hip_pitch_joint",
+                                      "right_hip_roll_joint",
+                                      "right_knee_joint",
+                                      "right_ankle_pitch_joint",
+                                      "right_ankle_roll_joint",
+                                      "torso_joint",
+                                      "L_index_proximal_joint",
+                                      "L_index_intermediate_joint",
+                                      "L_middle_proximal_joint",
+                                      "L_middle_intermediate_joint",
+                                      "L_pinky_proximal_joint",
+                                      "L_pinky_intermediate_joint",
+                                      "L_ring_proximal_joint",
+                                      "L_ring_intermediate_joint",
+                                      "L_thumb_proximal_yaw_joint",
+                                      "L_thumb_proximal_pitch_joint",
+                                      "L_thumb_intermediate_joint",
+                                      "L_thumb_distal_joint",
+                                      "R_index_proximal_joint",
+                                      "R_index_intermediate_joint",
+                                      "R_middle_proximal_joint",
+                                      "R_middle_intermediate_joint",
+                                      "R_pinky_proximal_joint",
+                                      "R_pinky_intermediate_joint",
+                                      "R_ring_proximal_joint",
+                                      "R_ring_intermediate_joint",
+                                      "R_thumb_proximal_yaw_joint",
+                                      "R_thumb_proximal_pitch_joint",
+                                      "R_thumb_intermediate_joint",
+                                      "R_thumb_distal_joint"
+                                    ]
+
+        self.reduced_robot = self.robot.buildReducedRobot(
+            list_of_joints_to_lock=self.mixed_jointsToLockIDs,
+            reference_configuration=np.array([0.0] * self.robot.model.nq),
+        )
+
+        self.reduced_robot.model.addFrame(
+            pin.Frame('L_ee',
+                      self.reduced_robot.model.getJointId('left_wrist_yaw_joint'),
+                      pin.SE3(np.eye(3),
+                              np.array([0.05,0,0]).T),
+                      pin.FrameType.OP_FRAME)
+        )
+        
+        self.reduced_robot.model.addFrame(
+            pin.Frame('R_ee',
+                      self.reduced_robot.model.getJointId('right_wrist_yaw_joint'),
+                      pin.SE3(np.eye(3),
+                              np.array([0.05,0,0]).T),
+                      pin.FrameType.OP_FRAME)
+        )
+
+        # for i in range(self.reduced_robot.model.nframes):
+        #     frame = self.reduced_robot.model.frames[i]
+        #     frame_id = self.reduced_robot.model.getFrameId(frame.name)
+        #     print(f"Frame ID: {frame_id}, Name: {frame.name}")
+        
+        # Creating Casadi models and data for symbolic computing
+        self.cmodel = cpin.Model(self.reduced_robot.model)
+        self.cdata = self.cmodel.createData()
+
+        # Creating symbolic variables
+        self.cq = casadi.SX.sym("q", self.reduced_robot.model.nq, 1) 
+        self.cTf_l = casadi.SX.sym("tf_l", 4, 4)
+        self.cTf_r = casadi.SX.sym("tf_r", 4, 4)
+        cpin.framesForwardKinematics(self.cmodel, self.cdata, self.cq)
+
+        # Get the hand joint ID and define the error function
+        self.L_hand_id = self.reduced_robot.model.getFrameId("L_ee")
+        self.R_hand_id = self.reduced_robot.model.getFrameId("R_ee")
+
+        self.translational_error = casadi.Function(
+            "translational_error",
+            [self.cq, self.cTf_l, self.cTf_r],
+            [
+                casadi.vertcat(
+                    self.cdata.oMf[self.L_hand_id].translation - self.cTf_l[:3,3],
+                    self.cdata.oMf[self.R_hand_id].translation - self.cTf_r[:3,3]
+                )
+            ],
+        )
+        self.rotational_error = casadi.Function(
+            "rotational_error",
+            [self.cq, self.cTf_l, self.cTf_r],
+            [
+                casadi.vertcat(
+                    cpin.log3(self.cdata.oMf[self.L_hand_id].rotation @ self.cTf_l[:3,:3].T),
+                    cpin.log3(self.cdata.oMf[self.R_hand_id].rotation @ self.cTf_r[:3,:3].T)
+                )
+            ],
+        )
+
+        # Defining the optimization problem
+        self.opti = casadi.Opti()
+        self.var_q = self.opti.variable(self.reduced_robot.model.nq)
+        self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq)   # for smooth
+        self.param_tf_l = self.opti.parameter(4, 4)
+        self.param_tf_r = self.opti.parameter(4, 4)
+        self.translational_cost = casadi.sumsqr(self.translational_error(self.var_q, self.param_tf_l, self.param_tf_r))
+        self.rotation_cost = casadi.sumsqr(self.rotational_error(self.var_q, self.param_tf_l, self.param_tf_r))
+        self.regularization_cost = casadi.sumsqr(self.var_q)
+        self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last)
+
+        # Setting optimization constraints and goals
+        self.opti.subject_to(self.opti.bounded(
+            self.reduced_robot.model.lowerPositionLimit,
+            self.var_q,
+            self.reduced_robot.model.upperPositionLimit)
+        )
+        self.opti.minimize(50 * self.translational_cost + self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost)
+
+        opts = {
+            'ipopt':{
+                'print_level':0,
+                'max_iter':50,
+                'tol':1e-6
+            },
+            'print_time':False,# print or not
+            'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F
+        }
+        self.opti.solver("ipopt", opts)
+
+        self.init_data = np.zeros(self.reduced_robot.model.nq)
+        self.smooth_filter = WeightedMovingFilter(np.array([0.4, 0.3, 0.2, 0.1]), 14)
+        self.vis = None
+
+        if self.Visualization:
+            # Initialize the Meshcat visualizer for visualization
+            self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model)
+            self.vis.initViewer(open=True) 
+            self.vis.loadViewerModel("pinocchio") 
+            self.vis.displayFrames(True, frame_ids=[101, 102], axis_length = 0.15, axis_width = 5)
+            self.vis.display(pin.neutral(self.reduced_robot.model))
+
+            # Enable the display of end effector target frames with short axis lengths and greater width.
+            frame_viz_names = ['L_ee_target', 'R_ee_target']
+            FRAME_AXIS_POSITIONS = (
+                np.array([[0, 0, 0], [1, 0, 0],
+                          [0, 0, 0], [0, 1, 0],
+                          [0, 0, 0], [0, 0, 1]]).astype(np.float32).T
+            )
+            FRAME_AXIS_COLORS = (
+                np.array([[1, 0, 0], [1, 0.6, 0],
+                          [0, 1, 0], [0.6, 1, 0],
+                          [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
+            )
+            axis_length = 0.1
+            axis_width = 10
+            for frame_viz_name in frame_viz_names:
+                self.vis.viewer[frame_viz_name].set_object(
+                    mg.LineSegments(
+                        mg.PointsGeometry(
+                            position=axis_length * FRAME_AXIS_POSITIONS,
+                            color=FRAME_AXIS_COLORS,
+                        ),
+                        mg.LineBasicMaterial(
+                            linewidth=axis_width,
+                            vertexColors=True,
+                        ),
+                    )
+                )
+    # If the robot arm is not the same size as your arm :)
+    def scale_arms(self, human_left_pose, human_right_pose, human_arm_length=0.60, robot_arm_length=0.75):
+        scale_factor = robot_arm_length / human_arm_length
+        robot_left_pose = human_left_pose.copy()
+        robot_right_pose = human_right_pose.copy()
+        robot_left_pose[:3, 3] *= scale_factor
+        robot_right_pose[:3, 3] *= scale_factor
+        return robot_left_pose, robot_right_pose
+
+    def solve_ik(self, left_wrist, right_wrist, current_lr_arm_motor_q = None, current_lr_arm_motor_dq = None):
+        if current_lr_arm_motor_q is not None:
+            self.init_data = current_lr_arm_motor_q
+        self.opti.set_initial(self.var_q, self.init_data)
+
+        left_wrist, right_wrist = self.scale_arms(left_wrist, right_wrist)
+        if self.Visualization:
+            self.vis.viewer['L_ee_target'].set_transform(left_wrist)   # for visualization
+            self.vis.viewer['R_ee_target'].set_transform(right_wrist)  # for visualization
+
+        self.opti.set_value(self.param_tf_l, left_wrist)
+        self.opti.set_value(self.param_tf_r, right_wrist)
+        self.opti.set_value(self.var_q_last, self.init_data) # for smooth
+
+        try:
+            sol = self.opti.solve()
+            # sol = self.opti.solve_limited()
+
+            sol_q = self.opti.value(self.var_q)
+            self.smooth_filter.add_data(sol_q)
+            sol_q = self.smooth_filter.filtered_data
+
+            if current_lr_arm_motor_dq is not None:
+                v = current_lr_arm_motor_dq * 0.0
+            else:
+                v = (sol_q - self.init_data) * 0.0
+
+            self.init_data = sol_q
+
+            sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv))
+
+            if self.Visualization:
+                self.vis.display(sol_q)  # for visualization
+
+            return sol_q, sol_tauff
+        
+        except Exception as e:
+            print(f"ERROR in convergence, plotting debug info.{e}")
+
+            sol_q = self.opti.debug.value(self.var_q)
+            self.smooth_filter.add_data(sol_q)
+            sol_q = self.smooth_filter.filtered_data
+
+            if current_lr_arm_motor_dq is not None:
+                v = current_lr_arm_motor_dq * 0.0
+            else:
+                v = (sol_q - self.init_data) * 0.0
+
+            self.init_data = sol_q
+
+            sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv))
+
+            print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}")
+            if self.Visualization:
+                self.vis.display(sol_q)  # for visualization
+
+            # return sol_q, sol_tauff
+            return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv)
+
 
 if __name__ == "__main__":
-    arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = True)
+    # arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = True)
+    arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = True)
 
     # initial positon
     L_tf_target = pin.SE3(

teleop/teleop_hand_and_arm.py

@@ -12,8 +12,8 @@ parent_dir = os.path.dirname(current_dir)
 sys.path.append(parent_dir)
 
 from teleop.open_television.tv_wrapper import TeleVisionWrapper
-from teleop.robot_control.robot_arm import G1_29_ArmController
-from teleop.robot_control.robot_arm_ik import G1_29_ArmIK
+from teleop.robot_control.robot_arm import G1_29_ArmController, H1_2_ArmController
+from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, H1_2_ArmIK
 from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_Controller
 from teleop.image_server.image_client import ImageClient
 from teleop.utils.episode_writer import EpisodeWriter
@@ -28,13 +28,14 @@ if __name__ == '__main__':
     parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data')
     parser.set_defaults(record = False)
 
-    parser.add_argument('--dex', action='store_true', help='Use dex3-1 hand')
-    parser.add_argument('--gripper', dest='dex', action='store_false', help='Use gripper')
-    parser.set_defaults(dex = True)
+    parser.add_argument('--arm', type=str, choices=['G1_29', 'H1_2'], default='G1_29', help='Select arm controller')
+
+    parser.add_argument('--hand', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select hand controller')
+
     args = parser.parse_args()
     print(f"args:{args}\n")
 
-    # image
+    # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit)
     img_config = {
         'fps': 30,
         'head_camera_type': 'opencv',
@@ -75,26 +76,37 @@ if __name__ == '__main__':
     image_receive_thread.daemon = True
     image_receive_thread.start()
 
-    # television and arm
+    # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device.
     tv_wrapper = TeleVisionWrapper(BINOCULAR, tv_img_shape, tv_img_shm.name)
-    arm_ctrl = G1_29_ArmController()
-    arm_ik = G1_29_ArmIK()
+
+    # arm
+    if args.arm == 'G1_29':
+        arm_ctrl = G1_29_ArmController()
+        arm_ik = G1_29_ArmIK()
+    elif args.arm == 'H1_2':
+        arm_ctrl = H1_2_ArmController()
+        arm_ik = H1_2_ArmIK()
 
     # hand
-    if args.dex:
+    if args.hand == "dex3":
         left_hand_array = Array('d', 75, lock = True)         # [input]
         right_hand_array = Array('d', 75, lock = True)        # [input]
         dual_hand_data_lock = Lock()
         dual_hand_state_array = Array('d', 14, lock = False)  # [output] current left, right hand state(14) data.
         dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data.
         hand_ctrl = Dex3_1_Controller(left_hand_array, right_hand_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)
-    else:
+    elif args.hand == "gripper":
         left_hand_array = Array('d', 75, lock=True)
         right_hand_array = Array('d', 75, lock=True)
         dual_gripper_data_lock = Lock()
         dual_gripper_state_array = Array('d', 2, lock=False)   # current left, right gripper state(2) data.
         dual_gripper_action_array = Array('d', 2, lock=False)  # current left, right gripper action(2) data.
         gripper_ctrl = Gripper_Controller(left_hand_array, right_hand_array, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array)
+    elif args.hand == "inspire1":
+        print("Inspire1_Controller comming soon.")
+        pass
+    else:
+        pass
     
     if args.record:
         recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True)