diff --git a/src/lerobot/robots/unitree_g1/assets/g1/.gitignore b/src/lerobot/robots/unitree_g1/assets/g1/.gitignore new file mode 100644 index 000000000..0a293e49d --- /dev/null +++ b/src/lerobot/robots/unitree_g1/assets/g1/.gitignore @@ -0,0 +1,2 @@ +*.gv +*.pdf diff --git a/src/lerobot/robots/unitree_g1/assets/g1/README.md b/src/lerobot/robots/unitree_g1/assets/g1/README.md new file mode 100644 index 000000000..f666cf98c --- /dev/null +++ b/src/lerobot/robots/unitree_g1/assets/g1/README.md @@ -0,0 +1,33 @@ +# Unitree G1 Description (URDF & MJCF) + +## Overview + +This package includes a universal humanoid robot description (URDF & MJCF) for the [Unitree G1](https://www.unitree.com/g1/), developed by [Unitree Robotics](https://www.unitree.com/). + +MJCF/URDF for the G1 robot: + +| MJCF/URDF file name | `mode_machine` | Hip roll reduction ratio | Update status | dof#leg | dof#waist | dof#arm | dof#hand | +| ----------------------------- | :------------: | :----------------------: | ------------- | :-----: | :-------: | :-----: | :------: | +| `g1_23dof` | 1 | 14.5 | Beta | 6*2 | 1 | 5*2 | 0 | +| `g1_29dof` | 2 | 14.5 | Beta | 6*2 | 3 | 7*2 | 0 | +| `g1_29dof_with_hand` | 2 | 14.5 | Beta | 6*2 | 3 | 7*2 | 7*2 | +| `g1_29dof_lock_waist` | 3 | 14.5 | Beta | 6*2 | 1 | 7*2 | 0 | +| `g1_23dof_rev_1_0` | 4 | 22.5 | Up-to-date | 6*2 | 1 | 5*2 | 0 | +| `g1_29dof_rev_1_0` | 5 | 22.5 | Up-to-date | 6*2 | 3 | 7*2 | 0 | +| `g1_29dof_with_hand_rev_1_0` | 5 | 22.5 | Up-to-date | 6*2 | 3 | 7*2 | 7*2 | +| `g1_29dof_lock_waist_rev_1_0` | 6 | 22.5 | Up-to-date | 6*2 | 1 | 7*2 | 0 | +| `g1_dual_arm` | 9 | null | Up-to-date | 0 | 0 | 7*2 | 0 | + +## 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/URDF model file (`g1_XXX.xml`/`g1_XXX.urdf`) to the MuJoCo Viewer. + +## Note for teleoperate +g1_body29_hand14 is modified from [g1_29dof_with_hand_rev_1_0](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf) diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/head_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/head_link.STL new file mode 100644 index 000000000..2ee5fba15 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/head_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_ankle_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_ankle_pitch_link.STL new file mode 100644 index 000000000..69de84901 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_ankle_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_ankle_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_ankle_roll_link.STL new file mode 100644 index 000000000..8864e9f98 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_ankle_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_elbow_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_elbow_link.STL new file mode 100644 index 000000000..1a96d99ba Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_elbow_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_index_0_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_index_0_link.STL new file mode 100644 index 000000000..8069369af Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_index_0_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_index_1_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_index_1_link.STL new file mode 100644 index 000000000..89d231d7e Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_index_1_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_middle_0_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_middle_0_link.STL new file mode 100644 index 000000000..8069369af Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_middle_0_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_middle_1_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_middle_1_link.STL new file mode 100644 index 000000000..89d231d7e Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_middle_1_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_palm_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_palm_link.STL new file mode 100644 index 000000000..7d595ed8f Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_palm_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_0_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_0_link.STL new file mode 100644 index 000000000..3028bb4d6 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_0_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_1_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_1_link.STL new file mode 100644 index 000000000..d1c080c86 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_1_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_2_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_2_link.STL new file mode 100644 index 000000000..8b32e9663 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hand_thumb_2_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_pitch_link.STL new file mode 100644 index 000000000..5b751c767 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_roll_link.STL new file mode 100644 index 000000000..778437ffe Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_yaw_link.STL new file mode 100644 index 000000000..383093ab9 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_hip_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_knee_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_knee_link.STL new file mode 100644 index 000000000..f2e98e54e Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_knee_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_rubber_hand.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_rubber_hand.STL new file mode 100644 index 000000000..c44830f1f Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_rubber_hand.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 000000000..e698311fb Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_roll_link.STL new file mode 100644 index 000000000..80bca84ac Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 000000000..281e69905 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_shoulder_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_pitch_link.STL new file mode 100644 index 000000000..82cc224a8 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_roll_link.STL new file mode 100644 index 000000000..f3c263a7a Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_roll_rubber_hand.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_roll_rubber_hand.STL new file mode 100644 index 000000000..8fa435b66 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_roll_rubber_hand.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_yaw_link.STL new file mode 100644 index 000000000..31be4fd45 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/left_wrist_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/logo_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/logo_link.STL new file mode 100644 index 000000000..e97920985 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/logo_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/pelvis.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/pelvis.STL new file mode 100644 index 000000000..691a779b9 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/pelvis.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/pelvis_contour_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/pelvis_contour_link.STL new file mode 100644 index 000000000..42434339a Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/pelvis_contour_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_ankle_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_ankle_pitch_link.STL new file mode 100644 index 000000000..e77d8a2fe Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_ankle_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_ankle_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_ankle_roll_link.STL new file mode 100644 index 000000000..d4261dd7c Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_ankle_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_elbow_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_elbow_link.STL new file mode 100644 index 000000000..f259e3812 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_elbow_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_index_0_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_index_0_link.STL new file mode 100644 index 000000000..f87ad3212 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_index_0_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_index_1_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_index_1_link.STL new file mode 100644 index 000000000..6dea51ad9 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_index_1_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_middle_0_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_middle_0_link.STL new file mode 100644 index 000000000..f87ad3212 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_middle_0_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_middle_1_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_middle_1_link.STL new file mode 100644 index 000000000..6dea51ad9 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_middle_1_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_palm_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_palm_link.STL new file mode 100644 index 000000000..5ae00a783 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_palm_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_0_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_0_link.STL new file mode 100644 index 000000000..1cae7f18e Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_0_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_1_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_1_link.STL new file mode 100644 index 000000000..c141fbf5a Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_1_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_2_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_2_link.STL new file mode 100644 index 000000000..e942923c5 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hand_thumb_2_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_pitch_link.STL new file mode 100644 index 000000000..998a0a0f5 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_roll_link.STL new file mode 100644 index 000000000..47b2eebdd Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_yaw_link.STL new file mode 100644 index 000000000..371856427 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_hip_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_knee_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_knee_link.STL new file mode 100644 index 000000000..76d21a3d8 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_knee_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_rubber_hand.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_rubber_hand.STL new file mode 100644 index 000000000..0aacffbb8 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_rubber_hand.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 000000000..3f5b4ed47 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_roll_link.STL new file mode 100644 index 000000000..179d61753 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 000000000..2ba6076a8 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_shoulder_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_pitch_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_pitch_link.STL new file mode 100644 index 000000000..da194543c Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_pitch_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_roll_link.STL new file mode 100644 index 000000000..26868d228 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_roll_rubber_hand.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_roll_rubber_hand.STL new file mode 100644 index 000000000..c365aa9d8 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_roll_rubber_hand.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_yaw_link.STL new file mode 100644 index 000000000..d78890286 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/right_wrist_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_L_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_L_link.STL new file mode 100644 index 000000000..75d82f578 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_L_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_L_rod_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_L_rod_link.STL new file mode 100644 index 000000000..6747f3f93 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_L_rod_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_R_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_R_link.STL new file mode 100644 index 000000000..5cb5958ba Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_R_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_R_rod_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_R_rod_link.STL new file mode 100644 index 000000000..95cf415f7 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_constraint_R_rod_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link.STL new file mode 100644 index 000000000..17745af9c Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link_23dof_rev_1_0.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link_23dof_rev_1_0.STL new file mode 100644 index 000000000..079aa8124 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link_23dof_rev_1_0.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link_rev_1_0.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link_rev_1_0.STL new file mode 100644 index 000000000..8a759a709 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/torso_link_rev_1_0.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_constraint_L.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_constraint_L.STL new file mode 100644 index 000000000..911410fa5 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_constraint_L.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_constraint_R.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_constraint_R.STL new file mode 100644 index 000000000..babe79492 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_constraint_R.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_roll_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_roll_link.STL new file mode 100644 index 000000000..65831abd2 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_roll_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_roll_link_rev_1_0.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_roll_link_rev_1_0.STL new file mode 100644 index 000000000..a64f330c5 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_roll_link_rev_1_0.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_support_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_support_link.STL new file mode 100644 index 000000000..63660fb6e Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_support_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_yaw_link.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_yaw_link.STL new file mode 100644 index 000000000..7d36b02cc Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_yaw_link.STL differ diff --git a/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_yaw_link_rev_1_0.STL b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_yaw_link_rev_1_0.STL new file mode 100644 index 000000000..0fabb63d3 Binary files /dev/null and b/src/lerobot/robots/unitree_g1/assets/g1/meshes/waist_yaw_link_rev_1_0.STL differ diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index 48a50fa70..c4b2ef354 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -26,7 +26,7 @@ from ..config import RobotConfig class UnitreeG1Config(RobotConfig): # id: str = "unitree_g1" motion_mode: bool = False - simulation_mode: bool = True + simulation_mode: bool = False kp_high = 40.0 kd_high = 3.0 kp_low = 80.0 diff --git a/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py b/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py index 8392fe12f..bb093bed9 100644 --- a/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py +++ b/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py @@ -7,7 +7,6 @@ from pinocchio import casadi as cpin from pinocchio.visualize import MeshcatVisualizer import os import sys -from lerobot.robots.unitree_g1.eval_robot.utils.weighted_moving_filter import WeightedMovingFilter import logging_mp @@ -16,6 +15,97 @@ parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__ sys.path.append(parent2_dir) +import numpy as np +import matplotlib.pyplot as plt + + +class WeightedMovingFilter: + def __init__(self, weights, data_size=14): + self._window_size = len(weights) + self._weights = np.array(weights) + # assert np.isclose(np.sum(self._weights), 1.0), "[WeightedMovingFilter] the sum of weights list must be 1.0!" + self._data_size = data_size + self._filtered_data = np.zeros(self._data_size) + self._data_queue = [] + + def _apply_filter(self): + if len(self._data_queue) < self._window_size: + return self._data_queue[-1] + + data_array = np.array(self._data_queue) + temp_filtered_data = np.zeros(self._data_size) + for i in range(self._data_size): + temp_filtered_data[i] = np.convolve(data_array[:, i], self._weights, mode="valid")[-1] + + return temp_filtered_data + + def add_data(self, new_data): + assert len(new_data) == self._data_size + + if len(self._data_queue) > 0 and np.array_equal(new_data, self._data_queue[-1]): + return # skip duplicate data + + if len(self._data_queue) >= self._window_size: + self._data_queue.pop(0) + + self._data_queue.append(new_data) + self._filtered_data = self._apply_filter() + + @property + def filtered_data(self): + return self._filtered_data + + +def visualize_filter_comparison(filter_params, steps): + import time + + t = np.linspace(0, 4 * np.pi, steps) + original_data = np.array( + [np.sin(t + i) + np.random.normal(0, 0.2, len(t)) for i in range(35)] + ).T # sin wave with noise, shape is [len(t), 35] + + plt.figure(figsize=(14, 10)) + + for idx, weights in enumerate(filter_params): + filter = WeightedMovingFilter(weights, 14) + data_2b_filtered = original_data.copy() + filtered_data = [] + + time1 = time.time() + + for i in range(steps): + filter.add_data(data_2b_filtered[i][13:27]) # step i, columns 13 to 26 (total:14) + data_2b_filtered[i][13:27] = filter.filtered_data + filtered_data.append(data_2b_filtered[i]) + + time2 = time.time() + print(f"filter_params:{filter_params[idx]}, time cosume:{time2 - time1}") + + filtered_data = np.array(filtered_data) + + # col0 should not 2b filtered + plt.subplot(len(filter_params), 2, idx * 2 + 1) + plt.plot(filtered_data[:, 0], label=f"Filtered (Window {filter._window_size})") + plt.plot(original_data[:, 0], "r--", label="Original", alpha=0.5) + plt.title("Joint 1 - Should not to be filtered.") + plt.xlabel("Step") + plt.ylabel("Value") + plt.legend() + + # col13 should 2b filtered + plt.subplot(len(filter_params), 2, idx * 2 + 2) + plt.plot(filtered_data[:, 13], label=f"Filtered (Window {filter._window_size})") + plt.plot(original_data[:, 13], "r--", label="Original", alpha=0.5) + plt.title(f"Joint 13 - Window {filter._window_size}, Weights {weights}") + plt.xlabel("Step") + plt.ylabel("Value") + plt.legend() + + plt.tight_layout() + plt.show() + + + class G1_29_ArmIK: def __init__(self, Unit_Test=False, Visualization=False): np.set_printoptions(precision=5, suppress=True, linewidth=200) @@ -25,11 +115,11 @@ class G1_29_ArmIK: if not self.Unit_Test: self.robot = pin.RobotWrapper.BuildFromURDF( - "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/" + "src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/assets/g1/" ) else: self.robot = pin.RobotWrapper.BuildFromURDF( - "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/" + "src/lerobot/robots/unitree_g1eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/assets/g1/" ) # for test self.mixed_jointsToLockIDs = [