diff --git a/README.md b/README.md
index f14ac0f..8a1908b 100644
--- a/README.md
+++ b/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.
diff --git a/README_zh-CN.md b/README_zh-CN.md
index 51f820a..6d0fbfd 100644
--- a/README_zh-CN.md
+++ b/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 🔎 单元测试
+此步骤用于验证环境是否正确安装。
+即将展现。
diff --git a/assets/h1_2/README.md b/assets/h1_2/README.md
new file mode 100644
index 0000000..6e8f8c5
--- /dev/null
+++ b/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/).
+
+
+
+
+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.
diff --git a/assets/h1_2/h1_2.urdf b/assets/h1_2/h1_2.urdf
new file mode 100644
index 0000000..c3ea576
--- /dev/null
+++ b/assets/h1_2/h1_2.urdf
@@ -0,0 +1,1623 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/assets/h1_2/h1_2.xml b/assets/h1_2/h1_2.xml
new file mode 100644
index 0000000..00ee6b0
--- /dev/null
+++ b/assets/h1_2/h1_2.xml
@@ -0,0 +1,438 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/assets/h1_2/meshes/L_hand_base_link.STL b/assets/h1_2/meshes/L_hand_base_link.STL
new file mode 100644
index 0000000..a2f67ee
Binary files /dev/null and b/assets/h1_2/meshes/L_hand_base_link.STL differ
diff --git a/assets/h1_2/meshes/L_index_intermediate.STL b/assets/h1_2/meshes/L_index_intermediate.STL
new file mode 100644
index 0000000..24a5a83
Binary files /dev/null and b/assets/h1_2/meshes/L_index_intermediate.STL differ
diff --git a/assets/h1_2/meshes/L_index_proximal.STL b/assets/h1_2/meshes/L_index_proximal.STL
new file mode 100644
index 0000000..f899431
Binary files /dev/null and b/assets/h1_2/meshes/L_index_proximal.STL differ
diff --git a/assets/h1_2/meshes/L_middle_intermediate.STL b/assets/h1_2/meshes/L_middle_intermediate.STL
new file mode 100644
index 0000000..c498cd7
Binary files /dev/null and b/assets/h1_2/meshes/L_middle_intermediate.STL differ
diff --git a/assets/h1_2/meshes/L_middle_proximal.STL b/assets/h1_2/meshes/L_middle_proximal.STL
new file mode 100644
index 0000000..8f0aca6
Binary files /dev/null and b/assets/h1_2/meshes/L_middle_proximal.STL differ
diff --git a/assets/h1_2/meshes/L_pinky_intermediate.STL b/assets/h1_2/meshes/L_pinky_intermediate.STL
new file mode 100644
index 0000000..6b9cdad
Binary files /dev/null and b/assets/h1_2/meshes/L_pinky_intermediate.STL differ
diff --git a/assets/h1_2/meshes/L_pinky_proximal.STL b/assets/h1_2/meshes/L_pinky_proximal.STL
new file mode 100644
index 0000000..ccb63bf
Binary files /dev/null and b/assets/h1_2/meshes/L_pinky_proximal.STL differ
diff --git a/assets/h1_2/meshes/L_ring_intermediate.STL b/assets/h1_2/meshes/L_ring_intermediate.STL
new file mode 100644
index 0000000..23c4efa
Binary files /dev/null and b/assets/h1_2/meshes/L_ring_intermediate.STL differ
diff --git a/assets/h1_2/meshes/L_ring_proximal.STL b/assets/h1_2/meshes/L_ring_proximal.STL
new file mode 100644
index 0000000..c3cabb9
Binary files /dev/null and b/assets/h1_2/meshes/L_ring_proximal.STL differ
diff --git a/assets/h1_2/meshes/L_thumb_distal.STL b/assets/h1_2/meshes/L_thumb_distal.STL
new file mode 100644
index 0000000..291a14f
Binary files /dev/null and b/assets/h1_2/meshes/L_thumb_distal.STL differ
diff --git a/assets/h1_2/meshes/L_thumb_intermediate.STL b/assets/h1_2/meshes/L_thumb_intermediate.STL
new file mode 100644
index 0000000..03a3425
Binary files /dev/null and b/assets/h1_2/meshes/L_thumb_intermediate.STL differ
diff --git a/assets/h1_2/meshes/L_thumb_proximal.STL b/assets/h1_2/meshes/L_thumb_proximal.STL
new file mode 100644
index 0000000..08071d3
Binary files /dev/null and b/assets/h1_2/meshes/L_thumb_proximal.STL differ
diff --git a/assets/h1_2/meshes/L_thumb_proximal_base.STL b/assets/h1_2/meshes/L_thumb_proximal_base.STL
new file mode 100644
index 0000000..9933a6c
Binary files /dev/null and b/assets/h1_2/meshes/L_thumb_proximal_base.STL differ
diff --git a/assets/h1_2/meshes/R_hand_base_link.STL b/assets/h1_2/meshes/R_hand_base_link.STL
new file mode 100644
index 0000000..3478a98
Binary files /dev/null and b/assets/h1_2/meshes/R_hand_base_link.STL differ
diff --git a/assets/h1_2/meshes/R_index_intermediate.STL b/assets/h1_2/meshes/R_index_intermediate.STL
new file mode 100644
index 0000000..0cf0f8d
Binary files /dev/null and b/assets/h1_2/meshes/R_index_intermediate.STL differ
diff --git a/assets/h1_2/meshes/R_index_proximal.STL b/assets/h1_2/meshes/R_index_proximal.STL
new file mode 100644
index 0000000..cbfc711
Binary files /dev/null and b/assets/h1_2/meshes/R_index_proximal.STL differ
diff --git a/assets/h1_2/meshes/R_middle_intermediate.STL b/assets/h1_2/meshes/R_middle_intermediate.STL
new file mode 100644
index 0000000..ebc2616
Binary files /dev/null and b/assets/h1_2/meshes/R_middle_intermediate.STL differ
diff --git a/assets/h1_2/meshes/R_middle_proximal.STL b/assets/h1_2/meshes/R_middle_proximal.STL
new file mode 100644
index 0000000..3f630a3
Binary files /dev/null and b/assets/h1_2/meshes/R_middle_proximal.STL differ
diff --git a/assets/h1_2/meshes/R_pinky_intermediate.STL b/assets/h1_2/meshes/R_pinky_intermediate.STL
new file mode 100644
index 0000000..c2cf58b
Binary files /dev/null and b/assets/h1_2/meshes/R_pinky_intermediate.STL differ
diff --git a/assets/h1_2/meshes/R_pinky_proximal.STL b/assets/h1_2/meshes/R_pinky_proximal.STL
new file mode 100644
index 0000000..d4710f1
Binary files /dev/null and b/assets/h1_2/meshes/R_pinky_proximal.STL differ
diff --git a/assets/h1_2/meshes/R_ring_intermediate.STL b/assets/h1_2/meshes/R_ring_intermediate.STL
new file mode 100644
index 0000000..5fed852
Binary files /dev/null and b/assets/h1_2/meshes/R_ring_intermediate.STL differ
diff --git a/assets/h1_2/meshes/R_ring_proximal.STL b/assets/h1_2/meshes/R_ring_proximal.STL
new file mode 100644
index 0000000..5c3703e
Binary files /dev/null and b/assets/h1_2/meshes/R_ring_proximal.STL differ
diff --git a/assets/h1_2/meshes/R_thumb_distal.STL b/assets/h1_2/meshes/R_thumb_distal.STL
new file mode 100644
index 0000000..13f98f8
Binary files /dev/null and b/assets/h1_2/meshes/R_thumb_distal.STL differ
diff --git a/assets/h1_2/meshes/R_thumb_intermediate.STL b/assets/h1_2/meshes/R_thumb_intermediate.STL
new file mode 100644
index 0000000..a42829f
Binary files /dev/null and b/assets/h1_2/meshes/R_thumb_intermediate.STL differ
diff --git a/assets/h1_2/meshes/R_thumb_proximal.STL b/assets/h1_2/meshes/R_thumb_proximal.STL
new file mode 100644
index 0000000..137dbde
Binary files /dev/null and b/assets/h1_2/meshes/R_thumb_proximal.STL differ
diff --git a/assets/h1_2/meshes/R_thumb_proximal_base.STL b/assets/h1_2/meshes/R_thumb_proximal_base.STL
new file mode 100644
index 0000000..30817a8
Binary files /dev/null and b/assets/h1_2/meshes/R_thumb_proximal_base.STL differ
diff --git a/assets/h1_2/meshes/left_ankle_A_link.STL b/assets/h1_2/meshes/left_ankle_A_link.STL
new file mode 100644
index 0000000..541cb71
Binary files /dev/null and b/assets/h1_2/meshes/left_ankle_A_link.STL differ
diff --git a/assets/h1_2/meshes/left_ankle_A_rod_link.STL b/assets/h1_2/meshes/left_ankle_A_rod_link.STL
new file mode 100644
index 0000000..281dc11
Binary files /dev/null and b/assets/h1_2/meshes/left_ankle_A_rod_link.STL differ
diff --git a/assets/h1_2/meshes/left_ankle_B_link.STL b/assets/h1_2/meshes/left_ankle_B_link.STL
new file mode 100644
index 0000000..cd54990
Binary files /dev/null and b/assets/h1_2/meshes/left_ankle_B_link.STL differ
diff --git a/assets/h1_2/meshes/left_ankle_B_rod_link.STL b/assets/h1_2/meshes/left_ankle_B_rod_link.STL
new file mode 100644
index 0000000..8d80e0a
Binary files /dev/null and b/assets/h1_2/meshes/left_ankle_B_rod_link.STL differ
diff --git a/assets/h1_2/meshes/left_ankle_pitch_link.STL b/assets/h1_2/meshes/left_ankle_pitch_link.STL
new file mode 100644
index 0000000..827fb2c
Binary files /dev/null and b/assets/h1_2/meshes/left_ankle_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/left_ankle_roll_link.STL b/assets/h1_2/meshes/left_ankle_roll_link.STL
new file mode 100644
index 0000000..5fe6027
Binary files /dev/null and b/assets/h1_2/meshes/left_ankle_roll_link.STL differ
diff --git a/assets/h1_2/meshes/left_elbow_pitch_link.STL b/assets/h1_2/meshes/left_elbow_pitch_link.STL
new file mode 100644
index 0000000..55ace3f
Binary files /dev/null and b/assets/h1_2/meshes/left_elbow_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/left_elbow_roll_link.STL b/assets/h1_2/meshes/left_elbow_roll_link.STL
new file mode 100644
index 0000000..112437b
Binary files /dev/null and b/assets/h1_2/meshes/left_elbow_roll_link.STL differ
diff --git a/assets/h1_2/meshes/left_hand_link.STL b/assets/h1_2/meshes/left_hand_link.STL
new file mode 100644
index 0000000..6ce2dcf
Binary files /dev/null and b/assets/h1_2/meshes/left_hand_link.STL differ
diff --git a/assets/h1_2/meshes/left_hip_pitch_link.STL b/assets/h1_2/meshes/left_hip_pitch_link.STL
new file mode 100644
index 0000000..924c0e5
Binary files /dev/null and b/assets/h1_2/meshes/left_hip_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/left_hip_roll_link.STL b/assets/h1_2/meshes/left_hip_roll_link.STL
new file mode 100644
index 0000000..a9353e9
Binary files /dev/null and b/assets/h1_2/meshes/left_hip_roll_link.STL differ
diff --git a/assets/h1_2/meshes/left_hip_yaw_link.STL b/assets/h1_2/meshes/left_hip_yaw_link.STL
new file mode 100644
index 0000000..2b97fd7
Binary files /dev/null and b/assets/h1_2/meshes/left_hip_yaw_link.STL differ
diff --git a/assets/h1_2/meshes/left_knee_link.STL b/assets/h1_2/meshes/left_knee_link.STL
new file mode 100644
index 0000000..fe89e75
Binary files /dev/null and b/assets/h1_2/meshes/left_knee_link.STL differ
diff --git a/assets/h1_2/meshes/left_shoulder_pitch_link.STL b/assets/h1_2/meshes/left_shoulder_pitch_link.STL
new file mode 100644
index 0000000..23289ce
Binary files /dev/null and b/assets/h1_2/meshes/left_shoulder_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/left_shoulder_roll_link.STL b/assets/h1_2/meshes/left_shoulder_roll_link.STL
new file mode 100644
index 0000000..a28d854
Binary files /dev/null and b/assets/h1_2/meshes/left_shoulder_roll_link.STL differ
diff --git a/assets/h1_2/meshes/left_shoulder_yaw_link.STL b/assets/h1_2/meshes/left_shoulder_yaw_link.STL
new file mode 100644
index 0000000..87cbbbc
Binary files /dev/null and b/assets/h1_2/meshes/left_shoulder_yaw_link.STL differ
diff --git a/assets/h1_2/meshes/left_wrist_pitch_link.STL b/assets/h1_2/meshes/left_wrist_pitch_link.STL
new file mode 100644
index 0000000..83d6d87
Binary files /dev/null and b/assets/h1_2/meshes/left_wrist_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/link11_L.STL b/assets/h1_2/meshes/link11_L.STL
new file mode 100644
index 0000000..9933a6c
Binary files /dev/null and b/assets/h1_2/meshes/link11_L.STL differ
diff --git a/assets/h1_2/meshes/link11_R.STL b/assets/h1_2/meshes/link11_R.STL
new file mode 100644
index 0000000..30817a8
Binary files /dev/null and b/assets/h1_2/meshes/link11_R.STL differ
diff --git a/assets/h1_2/meshes/link12_L.STL b/assets/h1_2/meshes/link12_L.STL
new file mode 100644
index 0000000..08071d3
Binary files /dev/null and b/assets/h1_2/meshes/link12_L.STL differ
diff --git a/assets/h1_2/meshes/link12_R.STL b/assets/h1_2/meshes/link12_R.STL
new file mode 100644
index 0000000..137dbde
Binary files /dev/null and b/assets/h1_2/meshes/link12_R.STL differ
diff --git a/assets/h1_2/meshes/link13_L.STL b/assets/h1_2/meshes/link13_L.STL
new file mode 100644
index 0000000..03a3425
Binary files /dev/null and b/assets/h1_2/meshes/link13_L.STL differ
diff --git a/assets/h1_2/meshes/link13_R.STL b/assets/h1_2/meshes/link13_R.STL
new file mode 100644
index 0000000..a42829f
Binary files /dev/null and b/assets/h1_2/meshes/link13_R.STL differ
diff --git a/assets/h1_2/meshes/link14_L.STL b/assets/h1_2/meshes/link14_L.STL
new file mode 100644
index 0000000..291a14f
Binary files /dev/null and b/assets/h1_2/meshes/link14_L.STL differ
diff --git a/assets/h1_2/meshes/link14_R.STL b/assets/h1_2/meshes/link14_R.STL
new file mode 100644
index 0000000..13f98f8
Binary files /dev/null and b/assets/h1_2/meshes/link14_R.STL differ
diff --git a/assets/h1_2/meshes/link15_L.STL b/assets/h1_2/meshes/link15_L.STL
new file mode 100644
index 0000000..f899431
Binary files /dev/null and b/assets/h1_2/meshes/link15_L.STL differ
diff --git a/assets/h1_2/meshes/link15_R.STL b/assets/h1_2/meshes/link15_R.STL
new file mode 100644
index 0000000..cbfc711
Binary files /dev/null and b/assets/h1_2/meshes/link15_R.STL differ
diff --git a/assets/h1_2/meshes/link16_L.STL b/assets/h1_2/meshes/link16_L.STL
new file mode 100644
index 0000000..24a5a83
Binary files /dev/null and b/assets/h1_2/meshes/link16_L.STL differ
diff --git a/assets/h1_2/meshes/link16_R.STL b/assets/h1_2/meshes/link16_R.STL
new file mode 100644
index 0000000..0cf0f8d
Binary files /dev/null and b/assets/h1_2/meshes/link16_R.STL differ
diff --git a/assets/h1_2/meshes/link17_L.STL b/assets/h1_2/meshes/link17_L.STL
new file mode 100644
index 0000000..8f0aca6
Binary files /dev/null and b/assets/h1_2/meshes/link17_L.STL differ
diff --git a/assets/h1_2/meshes/link17_R.STL b/assets/h1_2/meshes/link17_R.STL
new file mode 100644
index 0000000..3f630a3
Binary files /dev/null and b/assets/h1_2/meshes/link17_R.STL differ
diff --git a/assets/h1_2/meshes/link18_L.STL b/assets/h1_2/meshes/link18_L.STL
new file mode 100644
index 0000000..c498cd7
Binary files /dev/null and b/assets/h1_2/meshes/link18_L.STL differ
diff --git a/assets/h1_2/meshes/link18_R.STL b/assets/h1_2/meshes/link18_R.STL
new file mode 100644
index 0000000..ebc2616
Binary files /dev/null and b/assets/h1_2/meshes/link18_R.STL differ
diff --git a/assets/h1_2/meshes/link19_L.STL b/assets/h1_2/meshes/link19_L.STL
new file mode 100644
index 0000000..c3cabb9
Binary files /dev/null and b/assets/h1_2/meshes/link19_L.STL differ
diff --git a/assets/h1_2/meshes/link19_R.STL b/assets/h1_2/meshes/link19_R.STL
new file mode 100644
index 0000000..5c3703e
Binary files /dev/null and b/assets/h1_2/meshes/link19_R.STL differ
diff --git a/assets/h1_2/meshes/link20_L.STL b/assets/h1_2/meshes/link20_L.STL
new file mode 100644
index 0000000..23c4efa
Binary files /dev/null and b/assets/h1_2/meshes/link20_L.STL differ
diff --git a/assets/h1_2/meshes/link20_R.STL b/assets/h1_2/meshes/link20_R.STL
new file mode 100644
index 0000000..5fed852
Binary files /dev/null and b/assets/h1_2/meshes/link20_R.STL differ
diff --git a/assets/h1_2/meshes/link21_L.STL b/assets/h1_2/meshes/link21_L.STL
new file mode 100644
index 0000000..ccb63bf
Binary files /dev/null and b/assets/h1_2/meshes/link21_L.STL differ
diff --git a/assets/h1_2/meshes/link21_R.STL b/assets/h1_2/meshes/link21_R.STL
new file mode 100644
index 0000000..d4710f1
Binary files /dev/null and b/assets/h1_2/meshes/link21_R.STL differ
diff --git a/assets/h1_2/meshes/link22_L.STL b/assets/h1_2/meshes/link22_L.STL
new file mode 100644
index 0000000..6b9cdad
Binary files /dev/null and b/assets/h1_2/meshes/link22_L.STL differ
diff --git a/assets/h1_2/meshes/link22_R.STL b/assets/h1_2/meshes/link22_R.STL
new file mode 100644
index 0000000..c2cf58b
Binary files /dev/null and b/assets/h1_2/meshes/link22_R.STL differ
diff --git a/assets/h1_2/meshes/logo_link.STL b/assets/h1_2/meshes/logo_link.STL
new file mode 100644
index 0000000..c396d51
Binary files /dev/null and b/assets/h1_2/meshes/logo_link.STL differ
diff --git a/assets/h1_2/meshes/pelvis.STL b/assets/h1_2/meshes/pelvis.STL
new file mode 100644
index 0000000..05e5512
Binary files /dev/null and b/assets/h1_2/meshes/pelvis.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_A_link.STL b/assets/h1_2/meshes/right_ankle_A_link.STL
new file mode 100644
index 0000000..2ed1eaf
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_A_link.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_A_rod_link.STL b/assets/h1_2/meshes/right_ankle_A_rod_link.STL
new file mode 100644
index 0000000..1fc9acc
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_A_rod_link.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_B_link.STL b/assets/h1_2/meshes/right_ankle_B_link.STL
new file mode 100644
index 0000000..6563110
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_B_link.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_B_rod_link.STL b/assets/h1_2/meshes/right_ankle_B_rod_link.STL
new file mode 100644
index 0000000..bdaa919
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_B_rod_link.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_link.STL b/assets/h1_2/meshes/right_ankle_link.STL
new file mode 100644
index 0000000..7045cea
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_link.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_pitch_link.STL b/assets/h1_2/meshes/right_ankle_pitch_link.STL
new file mode 100644
index 0000000..827fb2c
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/right_ankle_roll_link.STL b/assets/h1_2/meshes/right_ankle_roll_link.STL
new file mode 100644
index 0000000..5fe6027
Binary files /dev/null and b/assets/h1_2/meshes/right_ankle_roll_link.STL differ
diff --git a/assets/h1_2/meshes/right_elbow_pitch_link.STL b/assets/h1_2/meshes/right_elbow_pitch_link.STL
new file mode 100644
index 0000000..70bf14d
Binary files /dev/null and b/assets/h1_2/meshes/right_elbow_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/right_elbow_roll_link.STL b/assets/h1_2/meshes/right_elbow_roll_link.STL
new file mode 100644
index 0000000..5f7f228
Binary files /dev/null and b/assets/h1_2/meshes/right_elbow_roll_link.STL differ
diff --git a/assets/h1_2/meshes/right_hand_link.STL b/assets/h1_2/meshes/right_hand_link.STL
new file mode 100644
index 0000000..6ce2dcf
Binary files /dev/null and b/assets/h1_2/meshes/right_hand_link.STL differ
diff --git a/assets/h1_2/meshes/right_hip_pitch_link.STL b/assets/h1_2/meshes/right_hip_pitch_link.STL
new file mode 100644
index 0000000..aa0b4c2
Binary files /dev/null and b/assets/h1_2/meshes/right_hip_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/right_hip_roll_link.STL b/assets/h1_2/meshes/right_hip_roll_link.STL
new file mode 100644
index 0000000..6032b95
Binary files /dev/null and b/assets/h1_2/meshes/right_hip_roll_link.STL differ
diff --git a/assets/h1_2/meshes/right_hip_yaw_link.STL b/assets/h1_2/meshes/right_hip_yaw_link.STL
new file mode 100644
index 0000000..5039005
Binary files /dev/null and b/assets/h1_2/meshes/right_hip_yaw_link.STL differ
diff --git a/assets/h1_2/meshes/right_knee_link.STL b/assets/h1_2/meshes/right_knee_link.STL
new file mode 100644
index 0000000..fe89e75
Binary files /dev/null and b/assets/h1_2/meshes/right_knee_link.STL differ
diff --git a/assets/h1_2/meshes/right_pitch_link.STL b/assets/h1_2/meshes/right_pitch_link.STL
new file mode 100644
index 0000000..f5276e9
Binary files /dev/null and b/assets/h1_2/meshes/right_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/right_shoulder_pitch_link.STL b/assets/h1_2/meshes/right_shoulder_pitch_link.STL
new file mode 100644
index 0000000..87974a3
Binary files /dev/null and b/assets/h1_2/meshes/right_shoulder_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/right_shoulder_roll_link.STL b/assets/h1_2/meshes/right_shoulder_roll_link.STL
new file mode 100644
index 0000000..cf9bfb5
Binary files /dev/null and b/assets/h1_2/meshes/right_shoulder_roll_link.STL differ
diff --git a/assets/h1_2/meshes/right_shoulder_yaw_link.STL b/assets/h1_2/meshes/right_shoulder_yaw_link.STL
new file mode 100644
index 0000000..701f379
Binary files /dev/null and b/assets/h1_2/meshes/right_shoulder_yaw_link.STL differ
diff --git a/assets/h1_2/meshes/right_wrist_pitch_link.STL b/assets/h1_2/meshes/right_wrist_pitch_link.STL
new file mode 100644
index 0000000..f21d1f0
Binary files /dev/null and b/assets/h1_2/meshes/right_wrist_pitch_link.STL differ
diff --git a/assets/h1_2/meshes/torso_link.STL b/assets/h1_2/meshes/torso_link.STL
new file mode 100644
index 0000000..37e1f60
Binary files /dev/null and b/assets/h1_2/meshes/torso_link.STL differ
diff --git a/assets/h1_2/meshes/wrist_yaw_link.STL b/assets/h1_2/meshes/wrist_yaw_link.STL
new file mode 100644
index 0000000..90643cf
Binary files /dev/null and b/assets/h1_2/meshes/wrist_yaw_link.STL differ
diff --git a/assets/h1_2/scene.xml b/assets/h1_2/scene.xml
new file mode 100644
index 0000000..c2375fe
--- /dev/null
+++ b/assets/h1_2/scene.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/teleop/image_server/image_server.py b/teleop/image_server/image_server.py
index 8328210..fa542ff 100644
--- a/teleop/image_server/image_server.py
+++ b/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)
diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py
index 13f3c5c..3443e95 100644
--- a/teleop/robot_control/robot_arm.py
+++ b/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:
diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py
index 601a96b..8d1392f 100644
--- a/teleop/robot_control/robot_arm_ik.py
+++ b/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(
diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py
index 40535f8..8fb7c52 100644
--- a/teleop/teleop_hand_and_arm.py
+++ b/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)
+