diff --git a/src/lerobot/cameras/zmq/camera_zmq.py b/src/lerobot/cameras/zmq/camera_zmq.py index 78c6dc53b..154e5c235 100644 --- a/src/lerobot/cameras/zmq/camera_zmq.py +++ b/src/lerobot/cameras/zmq/camera_zmq.py @@ -442,7 +442,7 @@ class ZMQCamera(Camera): self.thread = None self.stop_event = None - def async_read(self, timeout_ms: float = 200) -> NDArray[Any]: + def async_read(self, timeout_ms: float = 2000) -> NDArray[Any]: """ Reads the latest available frame asynchronously. @@ -452,7 +452,7 @@ class ZMQCamera(Camera): Args: timeout_ms: Maximum time in milliseconds to wait for a frame - to become available. Defaults to 200ms. + to become available. Defaults to 2000ms. Returns: np.ndarray: The latest captured frame as a NumPy array in the format diff --git a/src/lerobot/cameras/zmq/configuration_zmq.py b/src/lerobot/cameras/zmq/configuration_zmq.py index b03e80c80..a5b60574e 100644 --- a/src/lerobot/cameras/zmq/configuration_zmq.py +++ b/src/lerobot/cameras/zmq/configuration_zmq.py @@ -65,7 +65,7 @@ class ZMQCameraConfig(CameraConfig): port: int = 5554 camera_name: str = "zmq_camera" color_mode: ColorMode = ColorMode.RGB - timeout_ms: int = 1000 + timeout_ms: int = 5000 def __post_init__(self) -> None: if self.color_mode not in (ColorMode.RGB, ColorMode.BGR): diff --git a/src/lerobot/robots/unitree_g1/config_hope_jr.py b/src/lerobot/robots/unitree_g1/config_hope_jr.py deleted file mode 100644 index aec079511..000000000 --- a/src/lerobot/robots/unitree_g1/config_hope_jr.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2025 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dataclasses import dataclass, field - -from lerobot.cameras import CameraConfig - -from ..config import RobotConfig - - -@RobotConfig.register_subclass("hope_jr_arm") -@dataclass -class HopeJrArmConfig(RobotConfig): - port: str # Port to connect to the hand - disable_torque_on_disconnect: bool = True - - # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a dictionary that maps motor - # names to the max_relative_target value for that motor. - max_relative_target: float | dict[str, float] | None = None - - cameras: dict[str, CameraConfig] = field(default_factory=dict) diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index f4d6f4846..48a50fa70 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 = False + simulation_mode: bool = True kp_high = 40.0 kd_high = 3.0 kp_low = 80.0 @@ -45,8 +45,10 @@ class UnitreeG1Config(RobotConfig): gradual_start_time = None gradual_time = None + audio_client = True + freeze_body = False - gravity_compensation = True + gravity_compensation = False cameras: dict[str, CameraConfig] = field(default_factory=dict) diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/brainco.yml b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/brainco.yml deleted file mode 100644 index 59ae66d6a..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/brainco.yml +++ /dev/null @@ -1,61 +0,0 @@ -left: - type: DexPilot # or vector - urdf_path: brainco_hand/brainco_left.urdf - - # Target refers to the retargeting target, which is the robot hand - target_joint_names: - [ - "left_thumb_metacarpal_joint", - "left_thumb_proximal_joint", - "left_index_proximal_joint", - "left_middle_proximal_joint", - "left_ring_proximal_joint", - "left_pinky_proximal_joint", - ] - - # for DexPilot type - wrist_link_name: "base_link" - finger_tip_link_names: [ "left_thumb_tip", "left_index_tip", "left_middle_tip", "left_ring_tip", "left_pinky_tip" ] - # If you do not know exactly how it is used, please leave it to None for default. - target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] - - # for vector type - target_origin_link_names: ["base_link", "base_link", "base_link", "base_link", "base_link"] - target_task_link_names: [ "left_thumb_tip", "left_index_tip", "left_middle_tip", "left_ring_tip", "left_pinky_tip" ] - target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: 0.90 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency - low_pass_alpha: 0.2 - -right: - type: DexPilot # or vector - urdf_path: brainco_hand/brainco_right.urdf - - # Target refers to the retargeting target, which is the robot hand - target_joint_names: - [ - "right_thumb_metacarpal_joint", - "right_thumb_proximal_joint", - "right_index_proximal_joint", - "right_middle_proximal_joint", - "right_ring_proximal_joint", - "right_pinky_proximal_joint", - ] - # for DexPilot type - wrist_link_name: "base_link" - finger_tip_link_names: [ "right_thumb_tip", "right_index_tip", "right_middle_tip", "right_ring_tip", "right_pinky_tip" ] - target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] - - # for vector type - target_origin_link_names: ["base_link", "base_link", "base_link", "base_link", "base_link"] - target_task_link_names: [ "right_thumb_tip", "right_index_tip", "right_middle_tip", "right_ring_tip", "right_pinky_tip" ] - target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: 0.90 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency - low_pass_alpha: 0.2 diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_base_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_base_link.STL deleted file mode 100644 index f8aee1028..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_base_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_distal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_distal_Link.STL deleted file mode 100644 index 89d3a1b51..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_distal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_proximal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_proximal_Link.STL deleted file mode 100644 index 2ef94bf9c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_proximal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_tip_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_tip_Link.STL deleted file mode 100644 index f8720d689..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_index_tip_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_distal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_distal_Link.STL deleted file mode 100644 index 7028ace85..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_distal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_proximal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_proximal_Link.STL deleted file mode 100644 index 59b09b982..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_proximal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_tip_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_tip_Link.STL deleted file mode 100644 index e624cb346..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_middle_tip_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_distal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_distal_Link.STL deleted file mode 100644 index bb8831323..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_distal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_proximal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_proximal_Link.STL deleted file mode 100644 index 33ff5b7bf..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_proximal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_tip_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_tip_Link.STL deleted file mode 100644 index 1620243ad..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_pinky_tip_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_distal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_distal_Link.STL deleted file mode 100644 index 1ffa7186c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_distal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_proximal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_proximal_Link.STL deleted file mode 100644 index 26189dc0d..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_proximal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_tip_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_tip_Link.STL deleted file mode 100644 index a15a56164..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_ring_tip_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_distal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_distal_Link.STL deleted file mode 100644 index b676fc101..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_distal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_metacarpal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_metacarpal_Link.STL deleted file mode 100644 index 4797f18b0..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_metacarpal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_proximal_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_proximal_Link.STL deleted file mode 100644 index 667b69c65..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_proximal_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_tip_Link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_tip_Link.STL deleted file mode 100644 index a43f9e28c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/left_thumb_tip_Link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_base_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_base_link.STL deleted file mode 100644 index 62aeba8f7..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_base_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_distal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_distal_link.STL deleted file mode 100644 index 9e1f56f42..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_distal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_proximal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_proximal_link.STL deleted file mode 100644 index 81d4896c8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_proximal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_tip.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_tip.STL deleted file mode 100644 index b0008f80b..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_index_tip.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_distal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_distal_link.STL deleted file mode 100644 index 440e0d7cc..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_distal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_proximal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_proximal_link.STL deleted file mode 100644 index b13e2fc48..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_proximal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_tip.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_tip.STL deleted file mode 100644 index 80adde3e5..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_middle_tip.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_distal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_distal_link.STL deleted file mode 100644 index 158a36746..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_distal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_proximal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_proximal_link.STL deleted file mode 100644 index b9eea28dc..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_proximal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_tip.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_tip.STL deleted file mode 100644 index 05f82c381..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_pinky_tip.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_distal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_distal_link.STL deleted file mode 100644 index 267045fda..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_distal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_proximal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_proximal_link.STL deleted file mode 100644 index e8495243d..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_proximal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_tip.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_tip.STL deleted file mode 100644 index 40724b6ca..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_ring_tip.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_distal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_distal_link.STL deleted file mode 100644 index aaae3a425..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_distal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_metacarpal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_metacarpal_link.STL deleted file mode 100644 index b7c637956..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_metacarpal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_proximal_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_proximal_link.STL deleted file mode 100644 index a8054f657..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_proximal_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_tip.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_tip.STL deleted file mode 100644 index 6715b1e54..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/brainco_hand/meshes/right_thumb_tip.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/.gitignore b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/.gitignore deleted file mode 100644 index 0a293e49d..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*.gv -*.pdf diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/README.md b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/README.md deleted file mode 100644 index f666cf98c..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/README.md +++ /dev/null @@ -1,33 +0,0 @@ -# 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/eval_robot/assets/g1/meshes/head_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/head_link.STL deleted file mode 100644 index 2ee5fba15..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/head_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_ankle_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_ankle_pitch_link.STL deleted file mode 100644 index 69de84901..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_ankle_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_ankle_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_ankle_roll_link.STL deleted file mode 100644 index 8864e9f98..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_ankle_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_elbow_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_elbow_link.STL deleted file mode 100644 index 1a96d99ba..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_elbow_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_index_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_index_0_link.STL deleted file mode 100644 index 8069369af..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_index_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_index_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_index_1_link.STL deleted file mode 100644 index 89d231d7e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_index_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_middle_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_middle_0_link.STL deleted file mode 100644 index 8069369af..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_middle_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_middle_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_middle_1_link.STL deleted file mode 100644 index 89d231d7e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_middle_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_palm_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_palm_link.STL deleted file mode 100644 index 7d595ed8f..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_palm_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_0_link.STL deleted file mode 100644 index 3028bb4d6..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_1_link.STL deleted file mode 100644 index d1c080c86..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_2_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_2_link.STL deleted file mode 100644 index 8b32e9663..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hand_thumb_2_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_pitch_link.STL deleted file mode 100644 index 5b751c767..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_roll_link.STL deleted file mode 100644 index 778437ffe..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_yaw_link.STL deleted file mode 100644 index 383093ab9..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_hip_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_knee_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_knee_link.STL deleted file mode 100644 index f2e98e54e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_knee_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_rubber_hand.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_rubber_hand.STL deleted file mode 100644 index c44830f1f..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_rubber_hand.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_pitch_link.STL deleted file mode 100644 index e698311fb..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_roll_link.STL deleted file mode 100644 index 80bca84ac..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_yaw_link.STL deleted file mode 100644 index 281e69905..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_shoulder_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_pitch_link.STL deleted file mode 100644 index 82cc224a8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_roll_link.STL deleted file mode 100644 index f3c263a7a..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_roll_rubber_hand.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_roll_rubber_hand.STL deleted file mode 100644 index 8fa435b66..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_roll_rubber_hand.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_yaw_link.STL deleted file mode 100644 index 31be4fd45..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/left_wrist_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/logo_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/logo_link.STL deleted file mode 100644 index e97920985..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/logo_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/pelvis.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/pelvis.STL deleted file mode 100644 index 691a779b9..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/pelvis.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/pelvis_contour_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/pelvis_contour_link.STL deleted file mode 100644 index 42434339a..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/pelvis_contour_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_ankle_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_ankle_pitch_link.STL deleted file mode 100644 index e77d8a2fe..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_ankle_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_ankle_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_ankle_roll_link.STL deleted file mode 100644 index d4261dd7c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_ankle_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_elbow_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_elbow_link.STL deleted file mode 100644 index f259e3812..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_elbow_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_index_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_index_0_link.STL deleted file mode 100644 index f87ad3212..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_index_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_index_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_index_1_link.STL deleted file mode 100644 index 6dea51ad9..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_index_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_middle_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_middle_0_link.STL deleted file mode 100644 index f87ad3212..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_middle_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_middle_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_middle_1_link.STL deleted file mode 100644 index 6dea51ad9..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_middle_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_palm_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_palm_link.STL deleted file mode 100644 index 5ae00a783..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_palm_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_0_link.STL deleted file mode 100644 index 1cae7f18e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_1_link.STL deleted file mode 100644 index c141fbf5a..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_2_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_2_link.STL deleted file mode 100644 index e942923c5..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hand_thumb_2_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_pitch_link.STL deleted file mode 100644 index 998a0a0f5..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_roll_link.STL deleted file mode 100644 index 47b2eebdd..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_yaw_link.STL deleted file mode 100644 index 371856427..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_hip_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_knee_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_knee_link.STL deleted file mode 100644 index 76d21a3d8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_knee_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_rubber_hand.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_rubber_hand.STL deleted file mode 100644 index 0aacffbb8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_rubber_hand.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_pitch_link.STL deleted file mode 100644 index 3f5b4ed47..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_roll_link.STL deleted file mode 100644 index 179d61753..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_yaw_link.STL deleted file mode 100644 index 2ba6076a8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_shoulder_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_pitch_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_pitch_link.STL deleted file mode 100644 index da194543c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_pitch_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_roll_link.STL deleted file mode 100644 index 26868d228..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_roll_rubber_hand.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_roll_rubber_hand.STL deleted file mode 100644 index c365aa9d8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_roll_rubber_hand.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_yaw_link.STL deleted file mode 100644 index d78890286..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/right_wrist_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_L_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_L_link.STL deleted file mode 100644 index 75d82f578..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_L_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_L_rod_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_L_rod_link.STL deleted file mode 100644 index 6747f3f93..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_L_rod_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_R_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_R_link.STL deleted file mode 100644 index 5cb5958ba..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_R_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_R_rod_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_R_rod_link.STL deleted file mode 100644 index 95cf415f7..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_constraint_R_rod_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link.STL deleted file mode 100644 index 17745af9c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link_23dof_rev_1_0.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link_23dof_rev_1_0.STL deleted file mode 100644 index 079aa8124..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link_23dof_rev_1_0.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link_rev_1_0.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link_rev_1_0.STL deleted file mode 100644 index 8a759a709..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/torso_link_rev_1_0.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_constraint_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_constraint_L.STL deleted file mode 100644 index 911410fa5..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_constraint_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_constraint_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_constraint_R.STL deleted file mode 100644 index babe79492..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_constraint_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_roll_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_roll_link.STL deleted file mode 100644 index 65831abd2..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_roll_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_roll_link_rev_1_0.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_roll_link_rev_1_0.STL deleted file mode 100644 index a64f330c5..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_roll_link_rev_1_0.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_support_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_support_link.STL deleted file mode 100644 index 63660fb6e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_support_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_yaw_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_yaw_link.STL deleted file mode 100644 index 7d36b02cc..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_yaw_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_yaw_link_rev_1_0.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_yaw_link_rev_1_0.STL deleted file mode 100644 index 0fabb63d3..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/g1/meshes/waist_yaw_link_rev_1_0.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/inspire_hand.yml b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/inspire_hand.yml deleted file mode 100644 index 8466ce08d..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/inspire_hand.yml +++ /dev/null @@ -1,62 +0,0 @@ -left: - type: DexPilot # or vector - urdf_path: inspire_hand/inspire_hand_left.urdf - - # Target refers to the retargeting target, which is the robot hand - target_joint_names: - [ - 'L_thumb_proximal_yaw_joint', - 'L_thumb_proximal_pitch_joint', - 'L_index_proximal_joint', - 'L_middle_proximal_joint', - 'L_ring_proximal_joint', - 'L_pinky_proximal_joint' - ] - - # for DexPilot type - wrist_link_name: "L_hand_base_link" - finger_tip_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] - # If you do not know exactly how it is used, please leave it to None for default. - target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] - - # for vector type - target_origin_link_names: [ "L_hand_base_link", "L_hand_base_link", "L_hand_base_link", "L_hand_base_link", "L_hand_base_link"] - target_task_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] - target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: 1.20 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency - low_pass_alpha: 0.2 - -right: - type: DexPilot # or vector - urdf_path: inspire_hand/inspire_hand_right.urdf - - # Target refers to the retargeting target, which is the robot hand - target_joint_names: - [ - 'R_thumb_proximal_yaw_joint', - 'R_thumb_proximal_pitch_joint', - 'R_index_proximal_joint', - 'R_middle_proximal_joint', - 'R_ring_proximal_joint', - 'R_pinky_proximal_joint' - ] - - # for DexPilot type - wrist_link_name: "R_hand_base_link" - finger_tip_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] - # If you do not know exactly how it is used, please leave it to None for - target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] - - target_origin_link_names: [ "R_hand_base_link", "R_hand_base_link", "R_hand_base_link", "R_hand_base_link", "R_hand_base_link"] - target_task_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] - target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: 1.20 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency - low_pass_alpha: 0.2 \ No newline at end of file diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/L_hand_base_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/L_hand_base_link.STL deleted file mode 100644 index a2f67eead..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/L_hand_base_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link11_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link11_L.STL deleted file mode 100644 index 9933a6ce1..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link11_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link11_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link11_R.STL deleted file mode 100644 index 30817a810..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link11_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link12_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link12_L.STL deleted file mode 100644 index 08071d3e6..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link12_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link12_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link12_R.STL deleted file mode 100644 index 137dbdef7..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link12_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link13_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link13_L.STL deleted file mode 100644 index 03a342581..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link13_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link13_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link13_R.STL deleted file mode 100644 index a42829f74..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link13_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link14_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link14_L.STL deleted file mode 100644 index 291a14f39..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link14_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link14_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link14_R.STL deleted file mode 100644 index 13f98f894..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link14_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link15_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link15_L.STL deleted file mode 100644 index f89943118..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link15_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link15_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link15_R.STL deleted file mode 100644 index cbfc7110e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link15_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link16_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link16_L.STL deleted file mode 100644 index 24a5a83d8..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link16_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link16_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link16_R.STL deleted file mode 100644 index 0cf0f8d1d..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link16_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link17_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link17_L.STL deleted file mode 100644 index 8f0aca60e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link17_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link17_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link17_R.STL deleted file mode 100644 index 3f630a303..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link17_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link18_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link18_L.STL deleted file mode 100644 index c498cd75c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link18_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link18_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link18_R.STL deleted file mode 100644 index ebc2616fd..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link18_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link19_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link19_L.STL deleted file mode 100644 index c3cabb926..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link19_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link19_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link19_R.STL deleted file mode 100644 index 5c3703ec2..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link19_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link20_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link20_L.STL deleted file mode 100644 index 23c4efa5a..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link20_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link20_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link20_R.STL deleted file mode 100644 index 5fed852fb..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link20_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link21_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link21_L.STL deleted file mode 100644 index ccb63bfc6..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link21_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link21_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link21_R.STL deleted file mode 100644 index d4710f126..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link21_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link22_L.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link22_L.STL deleted file mode 100644 index 6b9cdaddf..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link22_L.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link22_R.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link22_R.STL deleted file mode 100644 index c2cf58b3d..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/Link22_R.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/R_hand_base_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/R_hand_base_link.STL deleted file mode 100644 index 3478a981c..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/inspire_hand/meshes/R_hand_base_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_index_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_index_0_link.STL deleted file mode 100644 index 8069369af..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_index_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_index_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_index_1_link.STL deleted file mode 100644 index 89d231d7e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_index_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_middle_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_middle_0_link.STL deleted file mode 100644 index 8069369af..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_middle_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_middle_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_middle_1_link.STL deleted file mode 100644 index 89d231d7e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_middle_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_palm_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_palm_link.STL deleted file mode 100644 index 7d595ed8f..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_palm_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_0_link.STL deleted file mode 100644 index 3028bb4d6..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_1_link.STL deleted file mode 100644 index d1c080c86..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_2_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_2_link.STL deleted file mode 100644 index 8b32e9663..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/left_hand_thumb_2_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_index_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_index_0_link.STL deleted file mode 100644 index f87ad3212..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_index_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_index_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_index_1_link.STL deleted file mode 100644 index 6dea51ad9..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_index_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_middle_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_middle_0_link.STL deleted file mode 100644 index f87ad3212..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_middle_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_middle_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_middle_1_link.STL deleted file mode 100644 index 6dea51ad9..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_middle_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_palm_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_palm_link.STL deleted file mode 100644 index 5ae00a783..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_palm_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_0_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_0_link.STL deleted file mode 100644 index 1cae7f18e..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_0_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_1_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_1_link.STL deleted file mode 100644 index c141fbf5a..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_1_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_2_link.STL b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_2_link.STL deleted file mode 100644 index e942923c5..000000000 Binary files a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/meshes/right_hand_thumb_2_link.STL and /dev/null differ diff --git a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/unitree_dex3.yml b/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/unitree_dex3.yml deleted file mode 100644 index dc5ec854f..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/assets/unitree_hand/unitree_dex3.yml +++ /dev/null @@ -1,63 +0,0 @@ -left: - type: DexPilot # or vector - urdf_path: unitree_hand/unitree_dex3_left.urdf - - # Target refers to the retargeting target, which is the robot hand - target_joint_names: - [ - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - ] - - # for DexPilot type - wrist_link_name: "base_link" - finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip"] - # If you do not know exactly how it is used, please leave it to None for default. - target_link_human_indices_dexpilot: [[ 9, 14, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]] - - # for vector type - target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"] - target_task_link_names: ["thumb_tip","index_tip","middle_tip"] - target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]] - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: 1.0 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency - low_pass_alpha: 0.2 - -right: - type: DexPilot # or vector - urdf_path: unitree_hand/unitree_dex3_right.urdf - - # Target refers to the retargeting target, which is the robot hand - target_joint_names: - [ - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_joint", - ] - # for DexPilot type - wrist_link_name: "base_link" - finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip"] - target_link_human_indices_dexpilot: [[ 9, 14, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]] - - # for vector type - target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"] - target_task_link_names: ["thumb_tip", "index_tip", "middle_tip"] - target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]] - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: 1.0 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency - low_pass_alpha: 0.2 diff --git a/src/lerobot/robots/unitree_g1/eval_robot/bimanual_teleop.py b/src/lerobot/robots/unitree_g1/eval_robot/bimanual_teleop.py deleted file mode 100644 index 697c5d57d..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/bimanual_teleop.py +++ /dev/null @@ -1,316 +0,0 @@ -# teleop_to_robot_bimanual_with_cams_and_pedal.py -import os -import re -import time -import threading -import traceback -import numpy as np -import cv2 - -from evdev import InputDevice, categorize, ecodes - -from unitree_lerobot.lerobot.src.lerobot.teleoperators.homunculus import HomunculusArm, HomunculusArmConfig -from unitree_lerobot.eval_robot.robot_control.robot_arm_test import G1_29_ArmController, G1_29_JointIndex -from unitree_lerobot.eval_robot.robot_control.robot_arm_ik import G1_29_ArmIK -from unitree_lerobot.eval_robot.make_robot import setup_image_client - -# ------------ Config ------------ -PEDAL_DEV = "/dev/input/by-id/usb-PCsensor_FootSwitch-event-kbd" -EPISODE_ROOT = "box_task" -DISPLAY_SCALE_HEAD = (1280, 960) # ~2x per eye when binocular -DISPLAY_SCALE_WRIST = (960, 720) # ~2x per wrist view -LOG_HZ = 30.0 -CONTROL_HZ = 100.0 -# -------------------------------- - -# LEFT ARM order: S_pitch, S_yaw, S_roll, Elbow_flex, Wrist_roll -def scale_to_joint_limits_left(u5: np.ndarray) -> np.ndarray: - mins = np.array([-3.05, 0.00, -2.30, -1.00, 1.95], dtype=np.float32) - maxs = np.array([ 1.65, 1.20, 1.30, 1.00, -1.00], dtype=np.float32) - u = np.clip(u5.astype(np.float32), -1.0, 1.0) - return mins + (u + 1.0) * 0.5 * (maxs - mins) - -# RIGHT ARM order: S_pitch, S_yaw, S_roll, Elbow_flex, Wrist_roll -def scale_to_joint_limits_right(u5: np.ndarray) -> np.ndarray: - # (min > max for some joints is intentional to mirror orientation) - mins = np.array([-3.05, 0.00, 2.30, 2.00, -1.95], dtype=np.float32) - maxs = np.array([ 1.65, -1.20, -1.30, -1.00, 1.00], dtype=np.float32) - u = np.clip(u5.astype(np.float32), -1.0, 1.0) - return mins + (u + 1.0) * 0.5 * (maxs - mins) - -# ---- Episode numbering helpers ---- -_num_re = re.compile(r"^\d+$") - -def _ensure_root(): - os.makedirs(EPISODE_ROOT, exist_ok=True) - -def _next_episode_id() -> int: - _ensure_root() - existing = [int(d) for d in os.listdir(EPISODE_ROOT) - if _num_re.match(d) and os.path.isdir(os.path.join(EPISODE_ROOT, d))] - return (max(existing) + 1) if existing else 1 - -def _episode_dir(ep_id: int) -> str: - d = os.path.join(EPISODE_ROOT, str(ep_id)) - os.makedirs(d, exist_ok=True) - return d - -# ---- Pedal listener (runs in a thread) ---- -class PedalState: - def __init__(self): - self.recording = False - self.lock = threading.Lock() - self.start_trigger = False - self.stop_trigger = False - - def arm_start(self): - with self.lock: - self.start_trigger = True - - def arm_stop(self): - with self.lock: - self.stop_trigger = True - - def consume_triggers(self): - with self.lock: - s, t = self.start_trigger, self.stop_trigger - self.start_trigger = False - self.stop_trigger = False - return s, t - -def pedal_thread(dev_path: str, pstate: PedalState): - try: - dev = InputDevice(dev_path) - print(f"[Pedal] Using {dev.path} ({dev.name})") - for ev in dev.read_loop(): - if ev.type != ecodes.EV_KEY: - continue - key = categorize(ev) # key.keystate: 1=down, 0=up, 2=hold - code = getattr(key, "keycode", None) - state = getattr(key, "keystate", None) - if code == "KEY_A" and state == 1: - pstate.arm_start() - elif code == "KEY_B" and state == 1: - pstate.arm_stop() - except PermissionError as e: - print(f"[Pedal] Permission error opening {dev_path}: {e}") - print(" Try: sudo setfacl -m u:$USER:rw ") - except Exception as e: - print(f"[Pedal] Error: {e}") - traceback.print_exc() - -def main(): - # --- Teleop (Homunculus) --- - left_exo = HomunculusArm(HomunculusArmConfig("/dev/ttyACM0", id="unitree_left")) - right_exo = HomunculusArm(HomunculusArmConfig("/dev/ttyACM1", id="unitree_right")) - left_exo.connect(calibrate=True) - right_exo.connect(calibrate=True) - - # --- Robot --- - arm_ik = G1_29_ArmIK() - arm_ctrl = G1_29_ArmController(motion_mode=False, simulation_mode=False) - - # Zero non-arm joints ONCE - arm_joint_indices = set(range(15, 29)) # 15..28 are arms - for jid in G1_29_JointIndex: - if jid.value not in arm_joint_indices: - arm_ctrl.msg.motor_cmd[jid].mode = 1 - arm_ctrl.msg.motor_cmd[jid].q = 0.0 - arm_ctrl.msg.motor_cmd[jid].dq = 0.0 - arm_ctrl.msg.motor_cmd[jid].tau = 0.0 - - # --- Camera client --- - class _Cfg: - sim = False - arm = "G1_29" - ee = "dex3" - motion = False - image_info = setup_image_client(_Cfg) - tv_img_array = image_info["tv_img_array"] - wrist_img_array = image_info["wrist_img_array"] - is_binocular = image_info["is_binocular"] - has_wrist_cam = image_info["has_wrist_cam"] - - # --- Pedal listener thread --- - pstate = PedalState() - th = threading.Thread(target=pedal_thread, args=(PEDAL_DEV, pstate), daemon=True) - th.start() - - # --- Logging state --- - frames_rgb = [] # current episode frames (N, 640, 480, 3) RGB - obs_states = [] # current episode obs (N, 14) - recording = False - ep_id = None - log_dt = 1.0 / LOG_HZ - next_log_t = time.perf_counter() + log_dt - - try: - dt = 1.0 / CONTROL_HZ - k = 0 - - while True: - t0 = time.perf_counter() - - # Handle pedal triggers - start_trig, stop_trig = pstate.consume_triggers() - if start_trig and not recording: - ep_id = _next_episode_id() - _episode_dir(ep_id) - frames_rgb = [] - obs_states = [] - recording = True - print(f"[Episode] START -> {ep_id}") - - if stop_trig and recording: - # Save episode - try: - ep_dir = _episode_dir(ep_id) - if len(frames_rgb) > 0: - frames_np = np.stack(frames_rgb, axis=0) - np.savez_compressed(os.path.join(ep_dir, "frames_rgb_640x480.npz"), frames_np) - print(f"[Episode] Saved {frames_np.shape} frames to {ep_dir}/frames_rgb_640x480.npz") - if len(obs_states) > 0: - obs_np = np.stack(obs_states, axis=0).astype(np.float32) - np.savez_compressed(os.path.join(ep_dir, "obs_state.npz"), obs_np) - print(f"[Episode] Saved {obs_np.shape} obs to {ep_dir}/obs_state.npz") - except Exception: - traceback.print_exc() - finally: - frames_rgb = [] - obs_states = [] - recording = False - print(f"[Episode] STOP -> {ep_id}") - ep_id = None - - # --- LEFT teleop - teleop_l = left_exo.get_action() - vals_l = list(teleop_l.values())[:5] - vals_l = [vals_l[0]] + [-v for v in vals_l[1:]] - norm_l = np.asarray(vals_l, dtype=np.float32) / 100.0 - q_left = scale_to_joint_limits_left(norm_l) - - # --- RIGHT teleop - teleop_r = right_exo.get_action() - vals_r = list(teleop_r.values())[:5] - vals_r = [vals_r[0]] + [-v for v in vals_r[1:]] - norm_r = np.asarray(vals_r, dtype=np.float32) / 100.0 - q_right = scale_to_joint_limits_right(norm_r) - - # Build combined command (left: 0..4, right: 7..11) - arm_cmd = np.zeros(14, dtype=np.float32) - arm_cmd[0:5] = q_left - arm_cmd[7:12] = q_right - - # Send - tau = arm_ik.solve_tau(arm_cmd) - arm_ctrl.ctrl_dual_arm(arm_cmd, tau) - - # Readback for display/log - q_curr = None - try: - q_curr = arm_ctrl.get_current_dual_arm_q() # (14,) - if (k % 30) == 0: - qL = q_curr[0:5] - qR = q_curr[7:12] - print(f"[k={k:05d}] q_left={np.array2string(qL, precision=3)} q_right={np.array2string(qR, precision=3)}") - except Exception: - if (k % 300) == 0: - traceback.print_exc() - - # Camera display (bigger windows) - head_image = tv_img_array.copy() - if head_image is not None and head_image.size > 0 and np.any(head_image): - if is_binocular: - h, w = head_image.shape[:2] - left_head = head_image[:, :w // 2] - right_head = head_image[:, w // 2:] - cv2.imshow("Head Left", cv2.resize(left_head, DISPLAY_SCALE_HEAD)) - cv2.imshow("Head Right", cv2.resize(right_head, DISPLAY_SCALE_HEAD)) - else: - h, w = head_image.shape[:2] - # double-ish - cv2.imshow("Head", cv2.resize(head_image, (max(2*w // 1, 1), max(2*h // 1, 1)))) - - if has_wrist_cam and wrist_img_array is not None: - wrist_image = wrist_img_array.copy() - if wrist_image is not None and wrist_image.size > 0 and np.any(wrist_image): - h, w = wrist_image.shape[:2] - left_wrist = wrist_image[:, :w // 2] - right_wrist = wrist_image[:, w // 2:] - cv2.imshow("Wrist Left", cv2.resize(left_wrist, DISPLAY_SCALE_WRIST)) - cv2.imshow("Wrist Right", cv2.resize(right_wrist, DISPLAY_SCALE_WRIST)) - - # 30 Hz logging only when recording - now = time.perf_counter() - if recording and now >= next_log_t: - # Frame: pick head-left (or single head), convert BGR->RGB, resize 640x480 - if head_image is not None and head_image.size > 0 and np.any(head_image): - if is_binocular: - h, w = head_image.shape[:2] - src = head_image[:, :w // 2] # left eye - else: - src = head_image - frame_640x480 = cv2.resize(src, (640, 480)) - frame_rgb = cv2.cvtColor(frame_640x480, cv2.COLOR_BGR2RGB) - frames_rgb.append(frame_rgb) - - # Obs - if q_curr is not None: - obs_states.append(np.asarray(q_curr, dtype=np.float32)) - - # schedule next tick (keeps cadence stable even if we miss one) - next_log_t += (1.0 / LOG_HZ) - - # UI - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - print("Quit command received.") - break - elif key == ord('s'): - ts = time.strftime("%Y%m%d_%H%M%S") - if head_image is not None and head_image.size > 0: - cv2.imwrite(f"head_{ts}.jpg", head_image) - print(f"Saved head_{ts}.jpg") - - k += 1 - - # Loop pacing - elapsed = time.perf_counter() - t0 - if elapsed < dt: - time.sleep(dt - elapsed) - - except KeyboardInterrupt: - pass - except Exception: - traceback.print_exc() - finally: - # If recording when quitting, finalize the episode - if recording and ep_id is not None: - try: - ep_dir = _episode_dir(ep_id) - if len(frames_rgb) > 0: - frames_np = np.stack(frames_rgb, axis=0) - np.savez_compressed(os.path.join(ep_dir, "frames_rgb_640x480.npz"), frames_np) - print(f"[Episode] Saved {frames_np.shape} frames to {ep_dir}/frames_rgb_640x480.npz") - if len(obs_states) > 0: - obs_np = np.stack(obs_states, axis=0).astype(np.float32) - np.savez_compressed(os.path.join(ep_dir, "obs_state.npz"), obs_np) - print(f"[Episode] Saved {obs_np.shape} obs to {ep_dir}/obs_state.npz") - except Exception: - traceback.print_exc() - - # Cleanup OpenCV and shared resources - cv2.destroyAllWindows() - try: - from unitree_lerobot.eval_robot.utils.utils import cleanup_resources - cleanup_resources(image_info) - except Exception: - pass - for exo in (left_exo, right_exo): - try: - exo.disconnect() - except Exception: - pass - -if __name__ == "__main__": - main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/eval_g1.py b/src/lerobot/robots/unitree_g1/eval_robot/eval_g1.py deleted file mode 100644 index f63b9fbed..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/eval_g1.py +++ /dev/null @@ -1,190 +0,0 @@ -"""' -Refer to: lerobot/lerobot/scripts/eval.py - lerobot/lerobot/scripts/econtrol_robot.py - lerobot/robot_devices/control_utils.py -""" - -import time -import torch -import logging - -import numpy as np -from pprint import pformat -from dataclasses import asdict -from torch import nn -from contextlib import nullcontext - -from lerobot.policies.factory import make_policy -from lerobot.utils.utils import ( - get_safe_torch_device, - init_logging, -) -from lerobot.configs import parser -from lerobot.datasets.lerobot_dataset import LeRobotDataset -from multiprocessing.sharedctypes import SynchronizedArray - -from unitree_lerobot.eval_robot.make_robot import ( - setup_image_client, - setup_robot_interface, - process_images_and_observations, -) -from unitree_lerobot.eval_robot.utils.utils import ( - cleanup_resources, - predict_action, - to_list, - to_scalar, - EvalRealConfig, -) -from unitree_lerobot.eval_robot.utils.rerun_visualizer import RerunLogger, visualization_data - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def eval_policy( - cfg: EvalRealConfig, - policy: torch.nn.Module, - dataset: LeRobotDataset, -): - assert isinstance(policy, nn.Module), "Policy must be a PyTorch nn module." - - logger_mp.info(f"Arguments: {cfg}") - - if cfg.visualization: - rerun_logger = RerunLogger() - - policy.reset() # Set policy to evaluation mode - - image_info = None - try: - # --- Setup Phase --- - image_info = setup_image_client(cfg) - robot_interface = setup_robot_interface(cfg) - - # Unpack interfaces for convenience - arm_ctrl, arm_ik, ee_shared_mem, arm_dof, ee_dof = ( - robot_interface[key] for key in ["arm_ctrl", "arm_ik", "ee_shared_mem", "arm_dof", "ee_dof"] - ) - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam = ( - image_info[key] - for key in [ - "tv_img_array", - "wrist_img_array", - "tv_img_shape", - "wrist_img_shape", - "is_binocular", - "has_wrist_cam", - ] - ) - - # Get initial pose from the first step of the dataset - episode_idx = 0 - episode_info = dataset.meta.episodes[episode_idx] - - from_idx = episode_info["dataset_from_index"] - to_idx = episode_info["dataset_to_index"] - step = dataset[from_idx] - init_arm_pose = step["observation.state"][:arm_dof].cpu().numpy() - - user_input = input("Enter 's' to initialize the robot and start the evaluation: ") - idx = 0 - print(f"user_input: {user_input}") - full_state = None - if user_input.lower() == "s": - # "The initial positions of the robot's arm and fingers take the initial positions during data recording." - logger_mp.info("Initializing robot to starting pose...") - tau = robot_interface["arm_ik"].solve_tau(init_arm_pose) - robot_interface["arm_ctrl"].ctrl_dual_arm(init_arm_pose, tau) - time.sleep(1.0) # Give time for the robot to move - # --- Run Main Loop --- - logger_mp.info(f"Starting evaluation loop at {cfg.frequency} Hz.") - while True: - loop_start_time = time.perf_counter() - # 1. Get Observations - observation, current_arm_q = process_images_and_observations( - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam, arm_ctrl - ) - #convert wrist obs to tensors - observation["observation.images.cam_left_wrist"] = torch.from_numpy(observation["observation.images.cam_left_wrist"]) - observation["observation.images.cam_right_wrist"] = torch.from_numpy(observation["observation.images.cam_right_wrist"]) - left_ee_state = right_ee_state = np.array([]) - #if cfg.ee: - # with ee_shared_mem["lock"]: - # full_state = np.array(ee_shared_mem["state"][:]) - # left_ee_state = full_state[:ee_dof] - # right_ee_state = full_state[ee_dof:] - #pad with zeros - #left_ee_state = np.zeros(ee_dof) - #right_ee_state = np.zeros(ee_dof) - state_tensor = torch.from_numpy( - np.concatenate((current_arm_q, left_ee_state, right_ee_state), axis=0) - ).float() - observation["observation.state"] = state_tensor - # 2. Get Action from Policy - action = predict_action( - observation, - policy, - get_safe_torch_device(policy.config.device), - policy.config.use_amp, - step["task"], - use_dataset=cfg.use_dataset, - ) - action_np = action.cpu().numpy() - # 3. Execute Action - arm_action = action_np[:arm_dof]*0.1 - tau = arm_ik.solve_tau(arm_action) - arm_ctrl.ctrl_dual_arm(arm_action, tau) - - # if cfg.ee: - # ee_action_start_idx = arm_dof - # left_ee_action = action_np[ee_action_start_idx : ee_action_start_idx + ee_dof] - # right_ee_action = action_np[ee_action_start_idx + ee_dof : ee_action_start_idx + 2 * ee_dof] - # # logger_mp.info(f"EE Action: left {left_ee_action}, right {right_ee_action}") - - # if isinstance(ee_shared_mem["left"], SynchronizedArray): - # ee_shared_mem["left"][:] = to_list(left_ee_action) - # ee_shared_mem["right"][:] = to_list(right_ee_action) - # elif hasattr(ee_shared_mem["left"], "value") and hasattr(ee_shared_mem["right"], "value"): - # ee_shared_mem["left"].value = to_scalar(left_ee_action) - # ee_shared_mem["right"].value = to_scalar(right_ee_action) - - if cfg.visualization: - visualization_data(idx, observation, state_tensor.numpy(), action_np, rerun_logger) - idx += 1 - # Maintain frequency - time.sleep(max(0, (1.0 / cfg.frequency) - (time.perf_counter() - loop_start_time))) - except Exception as e: - logger_mp.info(f"An error occurred: {e}") - finally: - if image_info: - cleanup_resources(image_info) - - -@parser.wrap() -def eval_main(cfg: EvalRealConfig): - logging.info(pformat(asdict(cfg))) - - # Check device is available - device = get_safe_torch_device(cfg.policy.device, log=True) - - torch.backends.cudnn.benchmark = True - torch.backends.cuda.matmul.allow_tf32 = True - - logging.info("Making policy.") - - dataset = LeRobotDataset(repo_id=cfg.repo_id) - - policy = make_policy(cfg=cfg.policy, ds_meta=dataset.meta) - policy.eval() - - with torch.no_grad(), torch.autocast(device_type=device.type) if cfg.policy.use_amp else nullcontext(): - eval_policy(cfg=cfg, policy=policy, dataset=dataset) - - logging.info("End of eval") - - -if __name__ == "__main__": - init_logging() - eval_main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/eval_g1_dataset.py b/src/lerobot/robots/unitree_g1/eval_robot/eval_g1_dataset.py deleted file mode 100644 index 94abdd4e8..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/eval_g1_dataset.py +++ /dev/null @@ -1,182 +0,0 @@ -"""' -Refer to: lerobot/lerobot/scripts/eval.py - lerobot/lerobot/scripts/econtrol_robot.py - lerobot/robot_devices/control_utils.py -""" - -import torch -import tqdm -import logging -import time -import numpy as np -import matplotlib.pyplot as plt -from pprint import pformat -from dataclasses import asdict -from torch import nn -from contextlib import nullcontext -from lerobot.policies.factory import make_policy -from lerobot.utils.utils import ( - get_safe_torch_device, - init_logging, -) -from lerobot.configs import parser -from lerobot.datasets.lerobot_dataset import LeRobotDataset -from multiprocessing.sharedctypes import SynchronizedArray - -from unitree_lerobot.eval_robot.utils.utils import ( - extract_observation, - predict_action, - to_list, - to_scalar, - EvalRealConfig, -) -from unitree_lerobot.eval_robot.make_robot import setup_robot_interface -from unitree_lerobot.eval_robot.utils.rerun_visualizer import RerunLogger, visualization_data - - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def eval_policy( - cfg: EvalRealConfig, - policy: torch.nn.Module, - dataset: LeRobotDataset, -): - assert isinstance(policy, nn.Module), "Policy must be a PyTorch nn module." - - logger_mp.info(f"Arguments: {cfg}") - - if cfg.visualization: - rerun_logger = RerunLogger() - - policy.reset() # Set policy to evaluation mode - - # init pose - from_idx = dataset.episode_data_index["from"][0].item() - step = dataset[from_idx] - to_idx = dataset.episode_data_index["to"][0].item() - - ground_truth_actions = [] - predicted_actions = [] - - if cfg.send_real_robot: - robot_interface = setup_robot_interface(cfg) - arm_ctrl, arm_ik, ee_shared_mem, arm_dof, ee_dof = ( - robot_interface[key] for key in ["arm_ctrl", "arm_ik", "ee_shared_mem", "arm_dof", "ee_dof"] - ) - init_arm_pose = step["observation.state"][:arm_dof].cpu().numpy() - - # ===============init robot===================== - user_input = input("Please enter the start signal (enter 's' to start the subsequent program):") - if user_input.lower() == "s": - if cfg.send_real_robot: - # Initialize robot to starting pose - logger_mp.info("Initializing robot to starting pose...") - tau = robot_interface["arm_ik"].solve_tau(init_arm_pose) - robot_interface["arm_ctrl"].ctrl_dual_arm(init_arm_pose, tau) - - time.sleep(1) - - for step_idx in tqdm.tqdm(range(from_idx, to_idx)): - loop_start_time = time.perf_counter() - - step = dataset[step_idx] - observation = extract_observation(step) - - action = predict_action( - observation, - policy, - get_safe_torch_device(policy.config.device), - policy.config.use_amp, - step["task"], - use_dataset=True, - ) - action_np = action.cpu().numpy() - - ground_truth_actions.append(step["action"].numpy()) - predicted_actions.append(action_np) - - if cfg.send_real_robot: - # Execute Action - arm_action = action_np[:arm_dof] - tau = arm_ik.solve_tau(arm_action) - arm_ctrl.ctrl_dual_arm(arm_action, tau) - # logger_mp.info(f"Arm Action: {arm_action}") - - if cfg.ee: - ee_action_start_idx = arm_dof - left_ee_action = action_np[ee_action_start_idx : ee_action_start_idx + ee_dof] - right_ee_action = action_np[ee_action_start_idx + ee_dof : ee_action_start_idx + 2 * ee_dof] - # logger_mp.info(f"EE Action: left {left_ee_action}, right {right_ee_action}") - - if isinstance(ee_shared_mem["left"], SynchronizedArray): - ee_shared_mem["left"][:] = to_list(left_ee_action) - ee_shared_mem["right"][:] = to_list(right_ee_action) - elif hasattr(ee_shared_mem["left"], "value") and hasattr(ee_shared_mem["right"], "value"): - ee_shared_mem["left"].value = to_scalar(left_ee_action) - ee_shared_mem["right"].value = to_scalar(right_ee_action) - - if cfg.visualization: - visualization_data(step_idx, observation, observation["observation.state"], action_np, rerun_logger) - - # Maintain frequency - time.sleep(max(0, (1.0 / cfg.frequency) - (time.perf_counter() - loop_start_time))) - - ground_truth_actions = np.array(ground_truth_actions) - predicted_actions = np.array(predicted_actions) - - # Get the number of timesteps and action dimensions - n_timesteps, n_dims = ground_truth_actions.shape - - # Create a figure with subplots for each action dimension - fig, axes = plt.subplots(n_dims, 1, figsize=(12, 4 * n_dims), sharex=True) - fig.suptitle("Ground Truth vs Predicted Actions") - - # Plot each dimension - for i in range(n_dims): - ax = axes[i] if n_dims > 1 else axes - - ax.plot(ground_truth_actions[:, i], label="Ground Truth", color="blue") - ax.plot(predicted_actions[:, i], label="Predicted", color="red", linestyle="--") - ax.set_ylabel(f"Dim {i + 1}") - ax.legend() - - # Set common x-label - axes[-1].set_xlabel("Timestep") - - plt.tight_layout() - # plt.show() - - time.sleep(1) - plt.savefig("figure.png") - - -@parser.wrap() -def eval_main(cfg: EvalRealConfig): - logging.info(pformat(asdict(cfg))) - - # Check device is available - device = get_safe_torch_device(cfg.policy.device, log=True) - - torch.backends.cudnn.benchmark = True - torch.backends.cuda.matmul.allow_tf32 = True - - logging.info("Making policy.") - - dataset = LeRobotDataset(repo_id=cfg.repo_id) - - policy = make_policy(cfg=cfg.policy, ds_meta=dataset.meta) - policy.eval() - - with torch.no_grad(), torch.autocast(device_type=device.type) if cfg.policy.use_amp else nullcontext(): - eval_policy(cfg, policy, dataset) - - logging.info("End of eval") - - -if __name__ == "__main__": - init_logging() - eval_main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/eval_g1_sim.py b/src/lerobot/robots/unitree_g1/eval_robot/eval_g1_sim.py deleted file mode 100644 index 4d30e7650..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/eval_g1_sim.py +++ /dev/null @@ -1,259 +0,0 @@ -"""' -Refer to: lerobot/lerobot/scripts/eval.py - lerobot/lerobot/scripts/econtrol_robot.py - lerobot/robot_devices/control_utils.py -""" - -import time -import torch -import logging -import sys -import os - -import numpy as np - -# Suppress DDS debug output -class SuppressDDSOutput: - def __enter__(self): - self._original_stdout = sys.stdout - sys.stdout = open(os.devnull, 'w') - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - sys.stdout.close() - sys.stdout = self._original_stdout - - -from pprint import pformat -from dataclasses import asdict -from torch import nn -from contextlib import nullcontext - -from lerobot.policies.factory import make_policy -from lerobot.utils.utils import ( - get_safe_torch_device, - init_logging, -) -from lerobot.configs import parser -from lerobot.datasets.lerobot_dataset import LeRobotDataset -from multiprocessing.sharedctypes import SynchronizedArray - -from unitree_lerobot.eval_robot.make_robot import ( - setup_image_client, - setup_robot_interface, - process_images_and_observations, -) -from unitree_lerobot.eval_robot.utils.utils import ( - cleanup_resources, - predict_action, - to_list, - to_scalar, -) -from unitree_lerobot.eval_robot.utils.sim_savedata_utils import ( - EvalRealConfig, - process_data_add, - is_success, -) -from unitree_lerobot.eval_robot.utils.rerun_visualizer import RerunLogger, visualization_data - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def eval_policy( - cfg: EvalRealConfig, - policy: torch.nn.Module, - dataset: LeRobotDataset, -): - assert isinstance(policy, nn.Module), "Policy must be a PyTorch nn module." - - #logger_mp.info(f"Arguments: {cfg}") - - if cfg.visualization: - rerun_logger = RerunLogger() - - policy.reset() # Set policy to evaluation mode - - image_info = None - try: - # --- Setup Phase --- - image_info = setup_image_client(cfg) - - # Suppress DDS debug output during robot setup - robot_interface = setup_robot_interface(cfg) - # Unpack interfaces for convenience - ( - arm_ctrl, - arm_ik, - ee_shared_mem, - arm_dof, - ee_dof, - sim_state_subscriber, - sim_reward_subscriber, - episode_writer, - reset_pose_publisher, - ) = ( - robot_interface[key] - for key in [ - "arm_ctrl", - "arm_ik", - "ee_shared_mem", - "arm_dof", - "ee_dof", - "sim_state_subscriber", - "sim_reward_subscriber", - "episode_writer", - "reset_pose_publisher", - ] - ) - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam = ( - image_info[key] - for key in [ - "tv_img_array", - "wrist_img_array", - "tv_img_shape", - "wrist_img_shape", - "is_binocular", - "has_wrist_cam", - ] - ) - #is_binocular = True#add flag properly - - # Get initial pose from the first step of the dataset - from_idx = dataset.episode_data_index["from"][0].item() - step = dataset[from_idx] - init_arm_pose = step["observation.state"][:arm_dof].cpu().numpy() - - idx = 0 - full_state = None - - reward_stats = { - "reward_sum": 0.0, - "episode_num": 0.0, - } - - # "The initial positions of the robot's arm and fingers take the initial positions during data recording." - #logger_mp.info("Initializing robot to starting pose...") - tau = robot_interface["arm_ik"].solve_tau(init_arm_pose) - robot_interface["arm_ctrl"].ctrl_dual_arm(init_arm_pose, tau)#hits init pose just fine - time.sleep(1.0) # Give time for the robot to move - - # --- Run Main Loop --- - #logger_mp.info(f"Starting evaluation loop at {cfg.frequency} Hz.") - while True: - if cfg.save_data: - if reward_stats["episode_num"] == 0: - episode_writer.create_episode() - loop_start_time = time.perf_counter() - - # 1. Get Observations - observation, current_arm_q = process_images_and_observations( - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam, arm_ctrl - ) - - - left_ee_state = right_ee_state = np.array([]) - if cfg.ee: - with ee_shared_mem["lock"]: - full_state = np.array(ee_shared_mem["state"][:]) - left_ee_state = full_state[:ee_dof] - right_ee_state = full_state[ee_dof:] - state_tensor = torch.from_numpy( - np.concatenate((current_arm_q, left_ee_state, right_ee_state), axis=0) - ).float() - observation["observation.state"] = state_tensor - # 2. Get Action from Policy - #action_np = np.random.random(arm_dof) - action = predict_action( - observation, - policy, - get_safe_torch_device(policy.config.device), - policy.config.use_amp, - step["task"], - use_dataset=cfg.use_dataset, - ) - action_np = action.cpu().numpy() - # 3. Execute Action - arm_action = action_np[:arm_dof] - #arm action is random vector - - tau = arm_ik.solve_tau(arm_action) - - # Suppress DDS debug output during control commands - arm_ctrl.ctrl_dual_arm(arm_action, tau) - - if cfg.ee: - ee_action_start_idx = arm_dof - left_ee_action = action_np[ee_action_start_idx : ee_action_start_idx + ee_dof] - right_ee_action = action_np[ee_action_start_idx + ee_dof : ee_action_start_idx + 2 * ee_dof] - logger_mp.info(f"EE Action: left {left_ee_action}, right {right_ee_action}") - - if isinstance(ee_shared_mem["left"], SynchronizedArray): - ee_shared_mem["left"][:] = to_list(left_ee_action) - ee_shared_mem["right"][:] = to_list(right_ee_action) - elif hasattr(ee_shared_mem["left"], "value") and hasattr(ee_shared_mem["right"], "value"): - ee_shared_mem["left"].value = to_scalar(left_ee_action) - ee_shared_mem["right"].value = to_scalar(right_ee_action) - # save data - #if cfg.save_data: - # process_data_add(episode_writer, observation, current_arm_q, full_state, action, arm_dof, ee_dof) - - # is_success( - # sim_reward_subscriber, - # episode_writer, - # reset_pose_publisher, - # policy, - # cfg, - # reward_stats, - # init_arm_pose, - # robot_interface, - # ) - - if cfg.visualization: - visualization_data(idx, observation, state_tensor.numpy(), action_np, rerun_logger) - idx += 1 - reward_stats["episode_num"] = reward_stats["episode_num"] + 1 - # Maintain frequency - time.sleep(max(0, (1.0 / cfg.frequency) - (time.perf_counter() - loop_start_time))) - - except Exception as e: - logger_mp.info(f"An error occurred: {e}") - pass - finally: - if image_info: - cleanup_resources(image_info) - # Clean up sim state subscriber if it exists - if sim_state_subscriber and not getattr(sim_state_subscriber, "stopped", False): - sim_state_subscriber.stop_subscribe() - if sim_reward_subscriber and not getattr(sim_reward_subscriber, "stopped", False): - sim_reward_subscriber.stop_subscribe() - - -@parser.wrap() -def eval_main(cfg: EvalRealConfig): - logging.info(pformat(asdict(cfg))) - - # Check device is available - device = get_safe_torch_device(cfg.policy.device, log=True) - - torch.backends.cudnn.benchmark = True - torch.backends.cuda.matmul.allow_tf32 = True - - logging.info("Making policy.") - - dataset = LeRobotDataset(repo_id=cfg.repo_id) - - policy = make_policy(cfg=cfg.policy, ds_meta=dataset.meta) - policy.eval() - - with torch.no_grad(), torch.autocast(device_type=device.type) if cfg.policy.use_amp else nullcontext(): - eval_policy(cfg=cfg, policy=policy, dataset=dataset) - - logging.info("End of eval") - - -if __name__ == "__main__": - init_logging() - eval_main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/image_server/image_client.py b/src/lerobot/robots/unitree_g1/eval_robot/image_server/image_client.py deleted file mode 100644 index 8beeed459..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/image_server/image_client.py +++ /dev/null @@ -1,220 +0,0 @@ -import cv2 -import zmq -import numpy as np -import time -import struct -from collections import deque -from multiprocessing import shared_memory - - -class ImageClient: - def __init__( - self, - tv_img_shape=None, - tv_img_shm_name=None, - wrist_img_shape=None, - wrist_img_shm_name=None, - image_show=False, - server_address="192.168.123.164", - port=5554, - Unit_Test=False, - ): - """ - tv_img_shape: User's expected head camera resolution shape (H, W, C). It should match the output of the image service terminal. - tv_img_shm_name: Shared memory is used to easily transfer images across processes to the Vuer. - wrist_img_shape: User's expected wrist camera resolution shape (H, W, C). It should maintain the same shape as tv_img_shape. - wrist_img_shm_name: Shared memory is used to easily transfer images. - image_show: Whether to display received images in real time. - server_address: The ip address to execute the image server script. - port: The port number to bind to. It should be the same as the image server. - Unit_Test: When both server and client are True, it can be used to test the image transfer latency, \ - network jitter, frame loss rate and other information. - """ - self.running = True - self._image_show = image_show - self._server_address = server_address - self._port = port - - self.tv_img_shape = tv_img_shape - self.wrist_img_shape = wrist_img_shape - - self.tv_enable_shm = False - if self.tv_img_shape is not None and tv_img_shm_name is not None: - self.tv_image_shm = shared_memory.SharedMemory(name=tv_img_shm_name) - self.tv_img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=self.tv_image_shm.buf) - self.tv_enable_shm = True - - self.wrist_enable_shm = False - if self.wrist_img_shape is not None and wrist_img_shm_name is not None: - self.wrist_image_shm = shared_memory.SharedMemory(name=wrist_img_shm_name) - self.wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=self.wrist_image_shm.buf) - self.wrist_enable_shm = True - - # Performance evaluation parameters - self._enable_performance_eval = Unit_Test - if self._enable_performance_eval: - self._init_performance_metrics() - - def _init_performance_metrics(self): - self._frame_count = 0 # Total frames received - self._last_frame_id = -1 # Last received frame ID - - # Real-time FPS calculation using a time window - self._time_window = 1.0 # Time window size (in seconds) - self._frame_times = deque() # Timestamps of frames received within the time window - - # Data transmission quality metrics - self._latencies = deque() # Latencies of frames within the time window - self._lost_frames = 0 # Total lost frames - self._total_frames = 0 # Expected total frames based on frame IDs - - def cleanup(self): - """Clean up shared memory resources.""" - try: - if hasattr(self, 'tv_image_shm') and self.tv_image_shm: - self.tv_image_shm.close() - except Exception as e: - print(f"Warning: Failed to cleanup tv_image_shm: {e}") - - try: - if hasattr(self, 'wrist_image_shm') and self.wrist_image_shm: - self.wrist_image_shm.close() - except Exception as e: - print(f"Warning: Failed to cleanup wrist_image_shm: {e}") - - def __del__(self): - """Destructor""" - self.cleanup() - - def _update_performance_metrics(self, timestamp, frame_id, receive_time): - # Update latency - latency = receive_time - timestamp - self._latencies.append(latency) - - # Remove latencies outside the time window - while self._latencies and self._frame_times and self._latencies[0] < receive_time - self._time_window: - self._latencies.popleft() - - # Update frame times - self._frame_times.append(receive_time) - # Remove timestamps outside the time window - while self._frame_times and self._frame_times[0] < receive_time - self._time_window: - self._frame_times.popleft() - - # Update frame counts for lost frame calculation - expected_frame_id = self._last_frame_id + 1 if self._last_frame_id != -1 else frame_id - if frame_id != expected_frame_id: - lost = frame_id - expected_frame_id - if lost < 0: - print(f"[Image Client] Received out-of-order frame ID: {frame_id}") - else: - self._lost_frames += lost - print( - f"[Image Client] Detected lost frames: {lost}, Expected frame ID: {expected_frame_id}, Received frame ID: {frame_id}" - ) - self._last_frame_id = frame_id - self._total_frames = frame_id + 1 - - self._frame_count += 1 - - def _print_performance_metrics(self, receive_time): - if self._frame_count % 30 == 0: - # Calculate real-time FPS - real_time_fps = len(self._frame_times) / self._time_window if self._time_window > 0 else 0 - - # Calculate latency metrics - if self._latencies: - avg_latency = sum(self._latencies) / len(self._latencies) - max_latency = max(self._latencies) - min_latency = min(self._latencies) - jitter = max_latency - min_latency - else: - avg_latency = max_latency = min_latency = jitter = 0 - - # Calculate lost frame rate - lost_frame_rate = (self._lost_frames / self._total_frames) * 100 if self._total_frames > 0 else 0 - - print( - f"[Image Client] Real-time FPS: {real_time_fps:.2f}, Avg Latency: {avg_latency * 1000:.2f} ms, Max Latency: {max_latency * 1000:.2f} ms, \ - Min Latency: {min_latency * 1000:.2f} ms, Jitter: {jitter * 1000:.2f} ms, Lost Frame Rate: {lost_frame_rate:.2f}%" - ) - - def _close(self): - self._socket.close() - self._context.term() - if self._image_show: - cv2.destroyAllWindows() - print("Image client has been closed.") - - def receive_process(self): - # Set up ZeroMQ context and socket - self._context = zmq.Context() - self._socket = self._context.socket(zmq.SUB) - self._socket.connect(f"tcp://{self._server_address}:{self._port}") - self._socket.setsockopt_string(zmq.SUBSCRIBE, "") - - print("\nImage client has started, waiting to receive data...") - try: - while self.running: - # Receive message - message = self._socket.recv() - receive_time = time.time() - - if self._enable_performance_eval: - header_size = struct.calcsize("dI") - try: - # Attempt to extract header and image data - header = message[:header_size] - jpg_bytes = message[header_size:] - timestamp, frame_id = struct.unpack("dI", header) - except struct.error as e: - print(f"[Image Client] Error unpacking header: {e}, discarding message.") - continue - else: - # No header, entire message is image data - jpg_bytes = message - # Decode image - np_img = np.frombuffer(jpg_bytes, dtype=np.uint8) - current_image = cv2.imdecode(np_img, cv2.IMREAD_COLOR) - if current_image is None: - print("[Image Client] Failed to decode image.") - continue - - if self.tv_enable_shm: - np.copyto(self.tv_img_array, np.array(current_image[:, : self.tv_img_shape[1]])) - - if self.wrist_enable_shm: - np.copyto(self.wrist_img_array, np.array(current_image[:, -self.wrist_img_shape[1] :])) - - if self._image_show: - height, width = current_image.shape[:2] - resized_image = cv2.resize(current_image, (width // 2, height // 2)) - cv2.imshow("Image Client Stream", resized_image) - if cv2.waitKey(1) & 0xFF == ord("q"): - self.running = False - - if self._enable_performance_eval: - self._update_performance_metrics(timestamp, frame_id, receive_time) - self._print_performance_metrics(receive_time) - - except KeyboardInterrupt: - print("Image client interrupted by user.") - except Exception as e: - print(f"[Image Client] An error occurred while receiving data: {e}") - finally: - self._close() - - -if __name__ == "__main__": - # example1 - # tv_img_shape = (480, 1280, 3) - # img_shm = shared_memory.SharedMemory(create=True, size=np.prod(tv_img_shape) * np.uint8().itemsize) - # img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=img_shm.buf) - # img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = img_shm.name) - # img_client.receive_process() - - # example2 - # Initialize the client with performance evaluation enabled - # client = ImageClient(image_show = True, server_address='127.0.0.1', Unit_Test=True) # local test - client = ImageClient(image_show=True, server_address="192.168.123.164", Unit_Test=False) # deployment test - client.receive_process() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/image_server/image_server.py b/src/lerobot/robots/unitree_g1/eval_robot/image_server/image_server.py deleted file mode 100644 index 8fd5333e3..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/image_server/image_server.py +++ /dev/null @@ -1,347 +0,0 @@ -import cv2 -import zmq -import time -import struct -from collections import deque -import numpy as np -import pyrealsense2 as rs -import logging_mp - -logger_mp = logging_mp.get_logger(__name__, level=logging_mp.DEBUG) - - -class RealSenseCamera(object): - def __init__(self, img_shape, fps, serial_number=None, enable_depth=False) -> None: - """ - img_shape: [height, width] - serial_number: serial number - """ - self.img_shape = img_shape - self.fps = fps - self.serial_number = serial_number - self.enable_depth = enable_depth - - align_to = rs.stream.color - self.align = rs.align(align_to) - self.init_realsense() - - def init_realsense(self): - self.pipeline = rs.pipeline() - config = rs.config() - if self.serial_number is not None: - config.enable_device(self.serial_number) - - config.enable_stream(rs.stream.color, self.img_shape[1], self.img_shape[0], rs.format.bgr8, self.fps) - - if self.enable_depth: - config.enable_stream(rs.stream.depth, self.img_shape[1], self.img_shape[0], rs.format.z16, self.fps) - - profile = self.pipeline.start(config) - self._device = profile.get_device() - if self._device is None: - logger_mp.error("[Image Server] pipe_profile.get_device() is None .") - if self.enable_depth: - assert self._device is not None - depth_sensor = self._device.first_depth_sensor() - self.g_depth_scale = depth_sensor.get_depth_scale() - - self.intrinsics = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics() - - def get_frame(self): - frames = self.pipeline.wait_for_frames() - aligned_frames = self.align.process(frames) - color_frame = aligned_frames.get_color_frame() - - if self.enable_depth: - depth_frame = aligned_frames.get_depth_frame() - - if not color_frame: - return None - - color_image = np.asanyarray(color_frame.get_data()) - # color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB) - depth_image = np.asanyarray(depth_frame.get_data()) if self.enable_depth else None - return color_image, depth_image - - def release(self): - self.pipeline.stop() - - -class OpenCVCamera: - def __init__(self, device_id, img_shape, fps): - """ - decive_id: /dev/video* or * - img_shape: [height, width] - """ - self.id = device_id - self.fps = fps - self.img_shape = img_shape - self.cap = cv2.VideoCapture(self.id, cv2.CAP_V4L2) - self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc("M", "J", "P", "G")) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.img_shape[0]) - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.img_shape[1]) - self.cap.set(cv2.CAP_PROP_FPS, self.fps) - - # Test if the camera can read frames - if not self._can_read_frame(): - logger_mp.error( - f"[Image Server] Camera {self.id} Error: Failed to initialize the camera or read frames. Exiting..." - ) - self.release() - - def _can_read_frame(self): - success, _ = self.cap.read() - return success - - def release(self): - self.cap.release() - - def get_frame(self): - ret, color_image = self.cap.read() - if not ret: - return None - return color_image - - -class ImageServer: - def __init__(self, config, port=5554, Unit_Test=False): - """ - config example1: - { - '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"], # 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) - } - """ - logger_mp.info(config) - self.fps = config.get("fps", 30) - self.head_camera_type = config.get("head_camera_type", "opencv") - self.head_image_shape = config.get("head_camera_image_shape", [480, 640]) # (height, width) - self.head_camera_id_numbers = config.get("head_camera_id_numbers", [0]) - - self.wrist_camera_type = config.get("wrist_camera_type", None) - self.wrist_image_shape = config.get("wrist_camera_image_shape", [480, 640]) # (height, width) - self.wrist_camera_id_numbers = config.get("wrist_camera_id_numbers", None) - - self.port = port - self.Unit_Test = Unit_Test - - # Initialize head cameras - self.head_cameras = [] - if self.head_camera_type == "opencv": - for device_id in self.head_camera_id_numbers: - camera = OpenCVCamera(device_id=device_id, img_shape=self.head_image_shape, fps=self.fps) - self.head_cameras.append(camera) - elif self.head_camera_type == "realsense": - for serial_number in self.head_camera_id_numbers: - camera = RealSenseCamera(img_shape=self.head_image_shape, fps=self.fps, serial_number=serial_number) - self.head_cameras.append(camera) - else: - logger_mp.warning(f"[Image Server] Unsupported head_camera_type: {self.head_camera_type}") - - # Initialize wrist cameras if provided - self.wrist_cameras = [] - if self.wrist_camera_type and self.wrist_camera_id_numbers: - if self.wrist_camera_type == "opencv": - for device_id in self.wrist_camera_id_numbers: - camera = OpenCVCamera(device_id=device_id, img_shape=self.wrist_image_shape, fps=self.fps) - self.wrist_cameras.append(camera) - elif self.wrist_camera_type == "realsense": - for serial_number in self.wrist_camera_id_numbers: - camera = RealSenseCamera( - img_shape=self.wrist_image_shape, fps=self.fps, serial_number=serial_number - ) - self.wrist_cameras.append(camera) - else: - logger_mp.warning(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}") - - # Set ZeroMQ context and socket - self.context = zmq.Context() - self.socket = self.context.socket(zmq.PUB) - self.socket.bind(f"tcp://*:{self.port}") - - if self.Unit_Test: - self._init_performance_metrics() - - for cam in self.head_cameras: - if isinstance(cam, OpenCVCamera): - logger_mp.info( - f"[Image Server] Head camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}" - ) - elif isinstance(cam, RealSenseCamera): - logger_mp.info( - f"[Image Server] Head camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}" - ) - else: - logger_mp.warning("[Image Server] Unknown camera type in head_cameras.") - - for cam in self.wrist_cameras: - if isinstance(cam, OpenCVCamera): - logger_mp.info( - f"[Image Server] Wrist camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}" - ) - elif isinstance(cam, RealSenseCamera): - logger_mp.info( - f"[Image Server] Wrist camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}" - ) - else: - logger_mp.warning("[Image Server] Unknown camera type in wrist_cameras.") - - logger_mp.info("[Image Server] Image server has started, waiting for client connections...") - - def _init_performance_metrics(self): - self.frame_count = 0 # Total frames sent - self.time_window = 1.0 # Time window for FPS calculation (in seconds) - self.frame_times = deque() # Timestamps of frames sent within the time window - self.start_time = time.time() # Start time of the streaming - - def _update_performance_metrics(self, current_time): - # Add current time to frame times deque - self.frame_times.append(current_time) - # Remove timestamps outside the time window - while self.frame_times and self.frame_times[0] < current_time - self.time_window: - self.frame_times.popleft() - # Increment frame count - self.frame_count += 1 - - def _print_performance_metrics(self, current_time): - if self.frame_count % 30 == 0: - elapsed_time = current_time - self.start_time - real_time_fps = len(self.frame_times) / self.time_window - logger_mp.info( - f"[Image Server] Real-time FPS: {real_time_fps:.2f}, Total frames sent: {self.frame_count}, Elapsed time: {elapsed_time:.2f} sec" - ) - - def _close(self): - for cam in self.head_cameras: - cam.release() - for cam in self.wrist_cameras: - cam.release() - self.socket.close() - self.context.term() - logger_mp.info("[Image Server] The server has been closed.") - - def send_process(self): - try: - while True: - head_frames = [] - for cam in self.head_cameras: - if self.head_camera_type == "opencv": - color_image = cam.get_frame() - if color_image is None: - logger_mp.error("[Image Server] Head camera frame read is error.") - break - elif self.head_camera_type == "realsense": - color_image, depth_iamge = cam.get_frame() - if color_image is None: - logger_mp.error("[Image Server] Head camera frame read is error.") - break - head_frames.append(color_image) - if len(head_frames) != len(self.head_cameras): - break - head_color = cv2.hconcat(head_frames) - - if self.wrist_cameras: - wrist_frames = [] - for cam in self.wrist_cameras: - if self.wrist_camera_type == "opencv": - color_image = cam.get_frame() - if color_image is None: - logger_mp.error("[Image Server] Wrist camera frame read is error.") - break - elif self.wrist_camera_type == "realsense": - color_image, depth_iamge = cam.get_frame() - if color_image is None: - logger_mp.error("[Image Server] Wrist camera frame read is error.") - break - wrist_frames.append(color_image) - wrist_color = cv2.hconcat(wrist_frames) - - # Concatenate head and wrist frames - full_color = cv2.hconcat([head_color, wrist_color]) - else: - full_color = head_color - - ret, buffer = cv2.imencode(".jpg", full_color) - if not ret: - logger_mp.error("[Image Server] Frame imencode is failed.") - continue - - jpg_bytes = buffer.tobytes() - - if self.Unit_Test: - timestamp = time.time() - frame_id = self.frame_count - header = struct.pack("dI", timestamp, frame_id) # 8-byte double, 4-byte unsigned int - message = header + jpg_bytes - else: - message = jpg_bytes - - self.socket.send(message) - - if self.Unit_Test: - current_time = time.time() - self._update_performance_metrics(current_time) - self._print_performance_metrics(current_time) - - except KeyboardInterrupt: - logger_mp.warning("[Image Server] Interrupted by user.") - finally: - self._close() - - -if __name__ == "__main__": - # config = { - # "fps": 30, - # "head_camera_type": "opencv", - # "head_camera_image_shape": [480, 1280], # Head camera resolution - # "head_camera_id_numbers": [0], - # "wrist_camera_type": "opencv", - # "wrist_camera_image_shape": [480, 640], # Wrist camera resolution - # "wrist_camera_id_numbers": [2, 4], - # - #infrared - # config = { - # "fps": 30, - # "head_camera_type": "opencv", - # "head_camera_image_shape": [480, 640], - # "head_camera_id_numbers": [2], # <-- wrist cam that reported 480x640 - # # no wrist_* keys - # } - #rgb - config = { - "fps": 30, - "head_camera_type": "opencv", - "head_camera_image_shape": [480,1280], # match the device - "head_camera_id_numbers": [4], # /dev/video4 is RGB -} - - server = ImageServer(config, Unit_Test=False) - server.send_process() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/make_robot.py b/src/lerobot/robots/unitree_g1/eval_robot/make_robot.py deleted file mode 100644 index 9fe0f7622..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/make_robot.py +++ /dev/null @@ -1,298 +0,0 @@ -from multiprocessing import shared_memory, Value, Array, Lock -from typing import Any -import numpy as np -import argparse -import threading -import torch -from unitree_lerobot.eval_robot.image_server.image_client import ImageClient -from unitree_lerobot.eval_robot.robot_control.robot_arm import ( - G1_29_ArmController, - G1_23_ArmController, -) -from unitree_lerobot.eval_robot.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK -from unitree_lerobot.eval_robot.robot_control.robot_hand_unitree import ( - Dex3_1_Controller, - Dex1_1_Gripper_Controller, -) - -from unitree_lerobot.eval_robot.utils.episode_writer import EpisodeWriter - -from unitree_lerobot.eval_robot.robot_control.robot_hand_inspire import Inspire_Controller -from unitree_lerobot.eval_robot.robot_control.robot_hand_brainco import Brainco_Controller - - -from unitree_sdk2py.core.channel import ChannelPublisher -from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - -# Configuration for robot arms -ARM_CONFIG = { - "G1_29": {"controller": G1_29_ArmController, "ik_solver": G1_29_ArmIK, "dof": 14}, - "G1_23": {"controller": G1_23_ArmController, "ik_solver": G1_23_ArmIK, "dof": 14}, - # Add other arms here -} - -# Configuration for end-effectors -EE_CONFIG: dict[str, dict[str, Any]] = { - "dex3": { - "controller": Dex3_1_Controller, - "dof": 7, - "shared_mem_type": "Array", - "shared_mem_size": 7, - # "out_len": 14, - }, - "dex1": { - "controller": Dex1_1_Gripper_Controller, - "dof": 1, - "shared_mem_type": "Value", - # "out_len": 2, - }, - "inspire1": { - "controller": Inspire_Controller, - "dof": 6, - "shared_mem_type": "Array", - "shared_mem_size": 6, - # "out_len": 12, - }, - "brainco": { - "controller": Brainco_Controller, - "dof": 6, - "shared_mem_type": "Array", - "shared_mem_size": 6, - # "out_len": 12, - }, -} - - -def setup_image_client(args: argparse.Namespace) -> dict[str, Any]: - """Initializes and starts the image client and shared memory.""" - # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) - if getattr(args, "sim", False): - img_config = { - "fps": 30, - "head_camera_type": "opencv", - "head_camera_image_shape": [480, 640], # Head camera resolution - "head_camera_id_numbers": [0], - "wrist_camera_type": "opencv", - "wrist_camera_image_shape": [480, 640], # Wrist camera resolution - "wrist_camera_id_numbers": [2, 4], - } - else: - img_config = {#we dont have wrist for now - "fps": 30, - "head_camera_type": "opencv", - "head_camera_image_shape": [480, 640], # Head camera resolution - "head_camera_id_numbers": [0], - #"wrist_camera_type": "opencv", - #"wrist_camera_image_shape": [480, 640], # Wrist camera resolution - #"wrist_camera_id_numbers": [2, 4], - } - - ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular - if len(img_config["head_camera_id_numbers"]) > 1 or ( - img_config["head_camera_image_shape"][1] / img_config["head_camera_image_shape"][0] > ASPECT_RATIO_THRESHOLD - ): - BINOCULAR = True - else: - BINOCULAR = False - if "wrist_camera_type" in img_config: - WRIST = True - else: - WRIST = False - # BINOCULAR = True#add flag properly - if BINOCULAR and not ( - img_config["head_camera_image_shape"][1] / img_config["head_camera_image_shape"][0] > ASPECT_RATIO_THRESHOLD - ): - tv_img_shape = (img_config["head_camera_image_shape"][0], img_config["head_camera_image_shape"][1] * 2, 3) - else: - tv_img_shape = (img_config["head_camera_image_shape"][0], img_config["head_camera_image_shape"][1], 3) - - tv_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(tv_img_shape) * np.uint8().itemsize) - tv_img_array = np.ndarray(tv_img_shape, dtype=np.uint8, buffer=tv_img_shm.buf) - - # Initialize wrist camera variables - wrist_img_shm = None - wrist_img_array = None - wrist_img_shape = None - - if WRIST and getattr(args, "sim", False): - wrist_img_shape = (img_config["wrist_camera_image_shape"][0], img_config["wrist_camera_image_shape"][1] * 2, 3) - wrist_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(wrist_img_shape) * np.uint8().itemsize) - wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=wrist_img_shm.buf) - img_client = ImageClient( - tv_img_shape=tv_img_shape, - tv_img_shm_name=tv_img_shm.name, - wrist_img_shape=wrist_img_shape, - wrist_img_shm_name=wrist_img_shm.name, - server_address="127.0.0.1", - ) - elif WRIST and not getattr(args, "sim", False): - wrist_img_shape = (img_config["wrist_camera_image_shape"][0], img_config["wrist_camera_image_shape"][1] * 2, 3) - wrist_img_shm = shared_memory.SharedMemory(create=True, size=np.prod(wrist_img_shape) * np.uint8().itemsize) - wrist_img_array = np.ndarray(wrist_img_shape, dtype=np.uint8, buffer=wrist_img_shm.buf) - img_client = ImageClient( - tv_img_shape=tv_img_shape, - tv_img_shm_name=tv_img_shm.name, - wrist_img_shape=wrist_img_shape, - wrist_img_shm_name=wrist_img_shm.name, - ) - else: - img_client = ImageClient(tv_img_shape=tv_img_shape, tv_img_shm_name=tv_img_shm.name) - - has_wrist_cam = "wrist_camera_type" in img_config - - image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) - image_receive_thread.daemon = True - image_receive_thread.start() - - # Only include shared memory objects that exist - shm_resources = [tv_img_shm] - if wrist_img_shm is not None: - shm_resources.append(wrist_img_shm) - - return { - "tv_img_array": tv_img_array, - "wrist_img_array": wrist_img_array, - "tv_img_shape": tv_img_shape, - "wrist_img_shape": wrist_img_shape, - "is_binocular": BINOCULAR, - "has_wrist_cam": has_wrist_cam, - "shm_resources": shm_resources, - } - - -def _resolve_out_len(spec: dict[str, Any]) -> int: - return int(spec.get("out_len", 2 * int(spec["dof"]))) - - -def setup_robot_interface(args: argparse.Namespace) -> dict[str, Any]: - """ - Initializes robot controllers and IK solvers based on configuration. - """ - # ---------- Arm ---------- - arm_spec = ARM_CONFIG[args.arm] - arm_ik = arm_spec["ik_solver"]() - is_sim = getattr(args, "sim", False) - motion_mode = getattr(args, "motion", False) - arm_ctrl = arm_spec["controller"](motion_mode=motion_mode, simulation_mode=is_sim) - - result = { - "arm_ctrl": arm_ctrl, - "arm_ik": arm_ik, - "arm_dof": int(arm_spec["dof"]), - } - - # ---------- Simulation Components ---------- - if is_sim: - - from unitree_lerobot.eval_robot.utils.sim_state_topic import start_sim_state_subscribe, start_sim_reward_subscribe - from unitree_lerobot.eval_robot.utils.episode_writer import EpisodeWriter - from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize - from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ - - # Create simulation DDS subscribers - sim_state_subscriber = start_sim_state_subscribe() - sim_reward_subscriber = start_sim_reward_subscribe() - - # Initialize DDS channel 1 for simulation mode (if not already initialized) - try: - ChannelFactoryInitialize(1) - except: - pass # May already be initialized by arm controller - - # Create reset pose publisher (publishes to rt/reset_pose/cmd to match simulation) - reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) - reset_pose_publisher.Init() - - # Create episode writer for data collection - episode_writer = EpisodeWriter( - task_dir=args.task_dir if hasattr(args, 'task_dir') else "./data", - frequency=args.frequency if hasattr(args, 'frequency') else 30 - ) - - result.update({ - "sim_state_subscriber": sim_state_subscriber, - "sim_reward_subscriber": sim_reward_subscriber, - "reset_pose_publisher": reset_pose_publisher, - "episode_writer": episode_writer, - }) - - # ---------- End Effector ---------- - if hasattr(args, 'ee') and args.ee: - ee_spec = EE_CONFIG.get(args.ee) - if ee_spec: - result["ee_dof"] = int(ee_spec["dof"]) - # Add end effector shared memory setup here if needed - result["ee_shared_mem"] = {"lock": None, "state": [], "left": [], "right": []} - else: - result["ee_dof"] = 0 - result["ee_shared_mem"] = {"lock": None, "state": [], "left": [], "right": []} - else: - result["ee_dof"] = 0 - result["ee_shared_mem"] = {"lock": None, "state": [], "left": [], "right": []} - - EE_DOF = 7 - left_in = Array("d", EE_DOF, lock=True) # desired left finger q targets - right_in = Array("d", EE_DOF, lock=True) # desired right finger q targets - - # outputs (states + echoed actions), both length 14 (left 7 + right 7) - dual_state_out = Array("d", 2*EE_DOF, lock=True) - dual_action_out = Array("d", 2*EE_DOF, lock=True) - dual_lock = Lock() - - if getattr(args, 'ee', None) == 'dex3': - from robot_control.robot_hand_unitree import Dex3_1_Controller - ctrl = Dex3_1_Controller( - left_hand_array_in=left_in, - right_hand_array_in=right_in, - dual_hand_data_lock=dual_lock, - dual_hand_state_array_out=dual_state_out, - dual_hand_action_array_out=dual_action_out, - fps=100.0, - Unit_Test=False, - simulation_mode=getattr(args, "sim", False), # True in sim - ) - result["ee_dof"] = EE_DOF - result["ee_shared_mem"] = { - "lock": dual_lock, - "state": dual_state_out, # 14-length: [L7, R7] - "left": left_in, # input commands for left hand - "right": right_in, # input commands for right hand - } - - return result - - -def process_images_and_observations( - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam, arm_ctrl -): - """Processes images and generates observations.""" - current_tv_image = tv_img_array.copy() - current_wrist_image = wrist_img_array.copy() if has_wrist_cam else None - - left_top_cam = current_tv_image[:, : tv_img_shape[1] // 2] if is_binocular else current_tv_image - right_top_cam = current_tv_image[:, tv_img_shape[1] // 2 :] if is_binocular else left_top_cam.copy() - - left_wrist_cam = right_wrist_cam = None - if has_wrist_cam and current_wrist_image is not None: - left_wrist_cam = current_wrist_image[:, : wrist_img_shape[1] // 2] - right_wrist_cam = current_wrist_image[:, wrist_img_shape[1] // 2 :] - observation = { - "observation.image": torch.from_numpy(left_top_cam), - "observation.images.cam_right_high": torch.from_numpy(right_top_cam), - "observation.images.cam_left_wrist": torch.from_numpy(left_wrist_cam) if has_wrist_cam else np.zeros((480, 640, 3)), - "observation.images.cam_right_wrist": torch.from_numpy(right_wrist_cam) if has_wrist_cam else np.zeros((480, 640, 3)), - } - current_arm_q = arm_ctrl.get_current_dual_arm_q() - - return observation, current_arm_q - - -def publish_reset_category(category: int, publisher): # Scene Reset signal - msg = String_(data=str(category)) - publisher.Write(msg) - logger_mp.info(f"published reset category: {category}") diff --git a/src/lerobot/robots/unitree_g1/eval_robot/replay_robot.py b/src/lerobot/robots/unitree_g1/eval_robot/replay_robot.py deleted file mode 100644 index 1edf47d75..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/replay_robot.py +++ /dev/null @@ -1,118 +0,0 @@ -"""' -Refer to: lerobot/lerobot/scripts/eval.py - lerobot/lerobot/scripts/econtrol_robot.py - lerobot/robot_devices/control_utils.py -""" - -import time -import numpy as np - -from multiprocessing.sharedctypes import SynchronizedArray -from lerobot.configs import parser -from lerobot.datasets.lerobot_dataset import LeRobotDataset -from unitree_lerobot.eval_robot.make_robot import ( - setup_image_client, - setup_robot_interface, - process_images_and_observations, -) -from unitree_lerobot.eval_robot.utils.utils import cleanup_resources, EvalRealConfig - -from unitree_lerobot.eval_robot.utils.rerun_visualizer import RerunLogger, visualization_data -from unitree_lerobot.eval_robot.utils.utils import to_list, to_scalar - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -@parser.wrap() -def replay_main(cfg: EvalRealConfig): - logger_mp.info(f"Arguments: {cfg}") - - if cfg.visualization: - rerun_logger = RerunLogger() - - image_info = setup_image_client(cfg) - robot_interface = setup_robot_interface(cfg) - - """The main control and evaluation loop.""" - # Unpack interfaces for convenience - arm_ctrl, arm_ik, ee_shared_mem, arm_dof, ee_dof = ( - robot_interface[key] for key in ["arm_ctrl", "arm_ik", "ee_shared_mem", "arm_dof", "ee_dof"] - ) - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam = ( - image_info[key] - for key in [ - "tv_img_array", - "wrist_img_array", - "tv_img_shape", - "wrist_img_shape", - "is_binocular", - "has_wrist_cam", - ] - ) - - logger_mp.info(f"Starting evaluation loop at {cfg.frequency} Hz.") - - dataset = LeRobotDataset(repo_id=cfg.repo_id, root=cfg.root, episodes=[cfg.episodes]) - actions = dataset.hf_dataset.select_columns("action") - - # init pose - from_idx = dataset.episode_data_index["from"][0].item() - step = dataset[from_idx] - init_left_arm_pose = step["observation.state"][:14].cpu().numpy() - - user_input = input("Please enter the start signal (enter 's' to start the subsequent program):") - if user_input.lower() == "s": - # "The initial positions of the robot's arm and fingers take the initial positions during data recording." - logger_mp.info("Initializing robot to starting pose...") - tau = arm_ik.solve_tau(init_left_arm_pose) - arm_ctrl.ctrl_dual_arm(init_left_arm_pose, tau) - time.sleep(1) - for idx in range(dataset.num_frames): - loop_start_time = time.perf_counter() - - left_ee_state = right_ee_state = np.array([]) - action_np = actions[idx]["action"].numpy() - - # exec action - arm_action = action_np[:arm_dof] - tau = arm_ik.solve_tau(arm_action) - arm_ctrl.ctrl_dual_arm(arm_action, tau) - logger_mp.info(f"arm_action {arm_action}, tau {tau}") - - if cfg.ee: - ee_action_start_idx = arm_dof - left_ee_action = action_np[ee_action_start_idx : ee_action_start_idx + ee_dof] - right_ee_action = action_np[ee_action_start_idx + ee_dof : ee_action_start_idx + 2 * ee_dof] - logger_mp.info(f"EE Action: left {left_ee_action}, right {right_ee_action}") - - with ee_shared_mem["lock"]: - full_state = np.array(ee_shared_mem["state"][:]) - left_ee_state = full_state[:ee_dof] - right_ee_state = full_state[ee_dof:] - - if isinstance(ee_shared_mem["left"], SynchronizedArray): - ee_shared_mem["left"][:] = to_list(left_ee_action) - ee_shared_mem["right"][:] = to_list(right_ee_action) - elif hasattr(ee_shared_mem["left"], "value") and hasattr(ee_shared_mem["right"], "value"): - ee_shared_mem["left"].value = to_scalar(left_ee_action) - ee_shared_mem["right"].value = to_scalar(right_ee_action) - - if cfg.visualization: - observation, current_arm_q = process_images_and_observations( - tv_img_array, wrist_img_array, tv_img_shape, wrist_img_shape, is_binocular, has_wrist_cam, arm_ctrl - ) - state = np.concatenate((current_arm_q, left_ee_state, right_ee_state)) - - visualization_data(idx, observation, state, action_np, rerun_logger) - - # Maintain frequency - time.sleep(max(0, (1.0 / cfg.frequency) - (time.perf_counter() - loop_start_time))) - - cleanup_resources(image_info) - - -if __name__ == "__main__": - replay_main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/replay_working.py b/src/lerobot/robots/unitree_g1/eval_robot/replay_working.py deleted file mode 100644 index 06addefd5..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/replay_working.py +++ /dev/null @@ -1,88 +0,0 @@ -"""' -Refer to: lerobot/lerobot/scripts/eval.py - lerobot/lerobot/scripts/econtrol_robot.py - lerobot/robot_devices/control_utils.py -""" - -import time -import numpy as np -import cv2 -from multiprocessing.sharedctypes import SynchronizedArray -from lerobot.configs import parser -from lerobot.datasets.lerobot_dataset import LeRobotDataset -from lerobot.robots.unitree_g1.eval_robot.make_robot import ( - setup_image_client, - setup_robot_interface, - process_images_and_observations, -) -from lerobot.robots.unitree_g1.eval_robot.utils.utils import cleanup_resources, EvalRealConfig - -from lerobot.robots.unitree_g1.eval_robot.utils.rerun_visualizer import RerunLogger, visualization_data -from lerobot.robots.unitree_g1.eval_robot.utils.utils import to_list, to_scalar -from lerobot.robots.unitree_g1.eval_robot.robot_control.robot_arm_test import ( - G1_29_ArmController, G1_29_JointIndex -) -import logging_mp -from lerobot.robots.unitree_g1.eval_robot.robot_control.robot_arm_ik import G1_29_ArmIK - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def replay_main(): - - #damp needs to be on? do i start the robot as well - - arm_ik = G1_29_ArmIK() - arm_ctrl = G1_29_ArmController(motion_mode=False, simulation_mode=False)#motors move here upon init. there's a bug where when closing python the motors die - #arm_ctrl.ctrl_dual_arm_go_home() - dataset = LeRobotDataset(repo_id='nepyope/unitree_box_push_30fps_actions_fix', root="", episodes=[0]) - actions = dataset.hf_dataset.select_columns("action") - print(actions) - episode_idx = 8 - episode_info = dataset.meta.episodes[episode_idx] - - from_idx = episode_info["dataset_from_index"] - to_idx = episode_info["dataset_to_index"] - step = dataset[from_idx] - init_left_arm_pose = step["observation.state"][:14].cpu().numpy() - tau = arm_ik.solve_tau(init_left_arm_pose) - #arm_ctrl.ctrl_dual_arm(init_left_arm_pose, tau) - print('ok') - - # Create config object for image client - import argparse - cfg = argparse.Namespace( - sim=False, # Real robot (not simulation) - arm="G1_29", - ee="dex3", - motion=False - ) - - #replay actions from the dataset - for idx in range(dataset.num_frames): - - arm_joint_indices = set(range(15, 29)) # 15–28 are arms - for jid in G1_29_JointIndex: - if jid.value not in arm_joint_indices: - arm_ctrl.msg.motor_cmd[jid].mode = 1 - arm_ctrl.msg.motor_cmd[jid].q = 0.0 - arm_ctrl.msg.motor_cmd[jid].dq = 0.0 - arm_ctrl.msg.motor_cmd[jid].tau = 0.0 - loop_start_time = time.perf_counter() - - action_np = actions[idx]["action"].numpy() - - # exec action - arm_action = action_np[:14] - tau = arm_ik.solve_tau(arm_action) - arm_ctrl.ctrl_dual_arm(arm_action, tau) - logger_mp.info(f"arm_action {arm_action}, tau {tau}") - - # Maintain frequency - time.sleep(max(0, (1.0 / 30) - (time.perf_counter() - loop_start_time))) - - #some thread issue idk motion_mode true gets rid of the shakes motion mode - -if __name__ == "__main__": - replay_main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm.py b/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm.py deleted file mode 100644 index 63e096cb3..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm.py +++ /dev/null @@ -1,1207 +0,0 @@ -import numpy as np -import threading -import time -from enum import IntEnum - -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds -from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as hg_LowCmd, LowState_ as hg_LowState # idl for g1, h1_2 -from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ -from unitree_sdk2py.utils.crc import CRC - -from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as go_LowCmd, LowState_ as go_LowState # idl for h1 -from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ - -import logging_mp - -logger_mp = logging_mp.get_logger(__name__) - -kTopicLowCommand_Debug = "rt/lowcmd" -kTopicLowCommand_Motion = "rt/arm_sdk" -kTopicLowState = "rt/lowstate" - -G1_29_Num_Motors = 35 -G1_23_Num_Motors = 35 -H1_2_Num_Motors = 35 -H1_Num_Motors = 20 - - -class MotorState: - def __init__(self): - self.q = None - self.dq = None - - -class G1_29_LowState: - def __init__(self): - self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)] - - -class G1_23_LowState: - def __init__(self): - self.motor_state = [MotorState() for _ in range(G1_23_Num_Motors)] - - -class H1_2_LowState: - def __init__(self): - self.motor_state = [MotorState() for _ in range(H1_2_Num_Motors)] - - -class H1_LowState: - def __init__(self): - self.motor_state = [MotorState() for _ in range(H1_Num_Motors)] - - -class DataBuffer: - def __init__(self): - self.data = None - self.lock = threading.Lock() - - def GetData(self): - with self.lock: - return self.data - - def SetData(self, data): - with self.lock: - self.data = data - - -class G1_29_ArmController: - def __init__(self, motion_mode=False, simulation_mode=False): - logger_mp.info("Initialize G1_29_ArmController...") - self.q_target = np.zeros(14) - self.tauff_target = np.zeros(14) - self.motion_mode = motion_mode - self.simulation_mode = simulation_mode - self.kp_high = 300.0 - self.kd_high = 3.0 - self.kp_low = 80.0 - self.kd_low = 3.0 - self.kp_wrist = 40.0 - self.kd_wrist = 1.5 - - 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 - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - if self.motion_mode: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd) - else: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) - self.lowcmd_publisher.Init() - self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_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.1) - logger_mp.warning("[G1_29_ArmController] Waiting to subscribe dds...") - logger_mp.info("[G1_29_ArmController] Subscribe dds ok.") - - # 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() - logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") - logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") - - arm_indices = set(member.value for member in G1_29_JointArmIndex) - for id in G1_29_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] - logger_mp.info("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() - - logger_mp.info("Initialize G1_29_ArmController OK!\n") - - def _subscribe_motor_state(self): - while True: - msg = self.lowstate_subscriber.Read() - if msg is not None: - lowstate = G1_29_LowState() - for id in range(G1_29_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): - if self.motion_mode: - self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0 - - while True: - start_time = time.time() - - with self.ctrl_lock: - arm_q_target = self.q_target - arm_tauff_target = self.tauff_target - - if self.simulation_mode: - cliped_arm_q_target = arm_q_target - else: - 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) - # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") - # logger_mp.debug(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 G1_29_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 G1_29_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 G1_29_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.""" - logger_mp.info("[G1_29_ArmController] ctrl_dual_arm_go_home start...") - max_attempts = 100 - current_attempts = 0 - 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 current_attempts < max_attempts: - current_q = self.get_current_dual_arm_q() - if np.all(np.abs(current_q) < tolerance): - if self.motion_mode: - for weight in np.linspace(1, 0, num=101): - self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight - time.sleep(0.02) - logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") - break - current_attempts += 1 - 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 = [ - G1_29_JointIndex.kLeftAnklePitch.value, - G1_29_JointIndex.kRightAnklePitch.value, - # Left arm - G1_29_JointIndex.kLeftShoulderPitch.value, - G1_29_JointIndex.kLeftShoulderRoll.value, - G1_29_JointIndex.kLeftShoulderYaw.value, - G1_29_JointIndex.kLeftElbow.value, - # Right arm - G1_29_JointIndex.kRightShoulderPitch.value, - G1_29_JointIndex.kRightShoulderRoll.value, - G1_29_JointIndex.kRightShoulderYaw.value, - G1_29_JointIndex.kRightElbow.value, - ] - return motor_index.value in weak_motors - - def _Is_wrist_motor(self, motor_index): - wrist_motors = [ - G1_29_JointIndex.kLeftWristRoll.value, - G1_29_JointIndex.kLeftWristPitch.value, - G1_29_JointIndex.kLeftWristyaw.value, - G1_29_JointIndex.kRightWristRoll.value, - G1_29_JointIndex.kRightWristPitch.value, - G1_29_JointIndex.kRightWristYaw.value, - ] - return motor_index.value in wrist_motors - - -class G1_29_JointArmIndex(IntEnum): - # Left arm - kLeftShoulderPitch = 15 - kLeftShoulderRoll = 16 - kLeftShoulderYaw = 17 - kLeftElbow = 18 - kLeftWristRoll = 19 - kLeftWristPitch = 20 - kLeftWristyaw = 21 - - # Right arm - kRightShoulderPitch = 22 - kRightShoulderRoll = 23 - kRightShoulderYaw = 24 - kRightElbow = 25 - kRightWristRoll = 26 - kRightWristPitch = 27 - kRightWristYaw = 28 - - -class G1_29_JointIndex(IntEnum): - # Left leg - kLeftHipPitch = 0 - kLeftHipRoll = 1 - kLeftHipYaw = 2 - kLeftKnee = 3 - kLeftAnklePitch = 4 - kLeftAnkleRoll = 5 - - # Right leg - kRightHipPitch = 6 - kRightHipRoll = 7 - kRightHipYaw = 8 - kRightKnee = 9 - kRightAnklePitch = 10 - kRightAnkleRoll = 11 - - kWaistYaw = 12 - kWaistRoll = 13 - kWaistPitch = 14 - - # Left arm - kLeftShoulderPitch = 15 - kLeftShoulderRoll = 16 - kLeftShoulderYaw = 17 - kLeftElbow = 18 - kLeftWristRoll = 19 - kLeftWristPitch = 20 - kLeftWristyaw = 21 - - # Right arm - kRightShoulderPitch = 22 - kRightShoulderRoll = 23 - kRightShoulderYaw = 24 - kRightElbow = 25 - kRightWristRoll = 26 - kRightWristPitch = 27 - kRightWristYaw = 28 - - # not used - kNotUsedJoint0 = 29 - kNotUsedJoint1 = 30 - kNotUsedJoint2 = 31 - kNotUsedJoint3 = 32 - kNotUsedJoint4 = 33 - kNotUsedJoint5 = 34 - - -class G1_23_ArmController: - def __init__(self, motion_mode=False, simulation_mode=False): - self.simulation_mode = simulation_mode - self.motion_mode = motion_mode - - logger_mp.info("Initialize G1_23_ArmController...") - self.q_target = np.zeros(10) - self.tauff_target = np.zeros(10) - - self.kp_high = 300.0 - self.kd_high = 3.0 - self.kp_low = 80.0 - self.kd_low = 3.0 - self.kp_wrist = 40.0 - self.kd_wrist = 1.5 - - 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 - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - if self.motion_mode: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd) - else: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) - self.lowcmd_publisher.Init() - self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_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.1) - logger_mp.warning("[G1_23_ArmController] Waiting to subscribe dds...") - logger_mp.info("[G1_23_ArmController] Subscribe dds ok.") - - # 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() - logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") - logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") - - arm_indices = set(member.value for member in G1_23_JointArmIndex) - for id in G1_23_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] - logger_mp.info("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() - - logger_mp.info("Initialize G1_23_ArmController OK!\n") - - def _subscribe_motor_state(self): - while True: - msg = self.lowstate_subscriber.Read() - if msg is not None: - lowstate = G1_23_LowState() - for id in range(G1_23_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): - if self.motion_mode: - self.msg.motor_cmd[G1_23_JointIndex.kNotUsedJoint0].q = 1.0 - - while True: - start_time = time.time() - - with self.ctrl_lock: - arm_q_target = self.q_target - arm_tauff_target = self.tauff_target - - if self.simulation_mode: - cliped_arm_q_target = arm_q_target - else: - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit=self.arm_velocity_limit) - - for idx, id in enumerate(G1_23_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) - # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") - # logger_mp.debug(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 G1_23_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 G1_23_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 G1_23_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.""" - logger_mp.info("[G1_23_ArmController] ctrl_dual_arm_go_home start...") - max_attempts = 100 - current_attempts = 0 - with self.ctrl_lock: - self.q_target = np.zeros(10) - # self.tauff_target = np.zeros(10) - tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements - while current_attempts < max_attempts: - current_q = self.get_current_dual_arm_q() - if np.all(np.abs(current_q) < tolerance): - if self.motion_mode: - for weight in np.linspace(1, 0, num=101): - self.msg.motor_cmd[G1_23_JointIndex.kNotUsedJoint0].q = weight - time.sleep(0.02) - logger_mp.info("[G1_23_ArmController] both arms have reached the home position.") - break - current_attempts += 1 - 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 = [ - G1_23_JointIndex.kLeftAnklePitch.value, - G1_23_JointIndex.kRightAnklePitch.value, - # Left arm - G1_23_JointIndex.kLeftShoulderPitch.value, - G1_23_JointIndex.kLeftShoulderRoll.value, - G1_23_JointIndex.kLeftShoulderYaw.value, - G1_23_JointIndex.kLeftElbow.value, - # Right arm - G1_23_JointIndex.kRightShoulderPitch.value, - G1_23_JointIndex.kRightShoulderRoll.value, - G1_23_JointIndex.kRightShoulderYaw.value, - G1_23_JointIndex.kRightElbow.value, - ] - return motor_index.value in weak_motors - - def _Is_wrist_motor(self, motor_index): - wrist_motors = [ - G1_23_JointIndex.kLeftWristRoll.value, - G1_23_JointIndex.kRightWristRoll.value, - ] - return motor_index.value in wrist_motors - - -class G1_23_JointArmIndex(IntEnum): - # Left arm - kLeftShoulderPitch = 15 - kLeftShoulderRoll = 16 - kLeftShoulderYaw = 17 - kLeftElbow = 18 - kLeftWristRoll = 19 - - # Right arm - kRightShoulderPitch = 22 - kRightShoulderRoll = 23 - kRightShoulderYaw = 24 - kRightElbow = 25 - kRightWristRoll = 26 - - -class G1_23_JointIndex(IntEnum): - # Left leg - kLeftHipPitch = 0 - kLeftHipRoll = 1 - kLeftHipYaw = 2 - kLeftKnee = 3 - kLeftAnklePitch = 4 - kLeftAnkleRoll = 5 - - # Right leg - kRightHipPitch = 6 - kRightHipRoll = 7 - kRightHipYaw = 8 - kRightKnee = 9 - kRightAnklePitch = 10 - kRightAnkleRoll = 11 - - kWaistYaw = 12 - kWaistRollNotUsed = 13 - kWaistPitchNotUsed = 14 - - # Left arm - kLeftShoulderPitch = 15 - kLeftShoulderRoll = 16 - kLeftShoulderYaw = 17 - kLeftElbow = 18 - kLeftWristRoll = 19 - kLeftWristPitchNotUsed = 20 - kLeftWristyawNotUsed = 21 - - # Right arm - kRightShoulderPitch = 22 - kRightShoulderRoll = 23 - kRightShoulderYaw = 24 - kRightElbow = 25 - kRightWristRoll = 26 - kRightWristPitchNotUsed = 27 - kRightWristYawNotUsed = 28 - - # not used - kNotUsedJoint0 = 29 - kNotUsedJoint1 = 30 - kNotUsedJoint2 = 31 - kNotUsedJoint3 = 32 - kNotUsedJoint4 = 33 - kNotUsedJoint5 = 34 - - -class H1_2_ArmController: - def __init__(self, simulation_mode=False): - self.simulation_mode = simulation_mode - - logger_mp.info("Initialize H1_2_ArmController...") - self.q_target = np.zeros(14) - self.tauff_target = np.zeros(14) - - self.kp_high = 300.0 - self.kd_high = 5.0 - self.kp_low = 140.0 - self.kd_low = 3.0 - self.kp_wrist = 50.0 - self.kd_wrist = 2.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 - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) - self.lowcmd_publisher.Init() - self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_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.1) - logger_mp.warning("[H1_2_ArmController] Waiting to subscribe dds...") - logger_mp.info("[H1_2_ArmController] Subscribe dds ok.") - - # 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() - logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") - logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("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] - logger_mp.info("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() - - logger_mp.info("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 - - if self.simulation_mode: - cliped_arm_q_target = arm_q_target - else: - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit=self.arm_velocity_limit) - - for idx, id in enumerate(H1_2_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) - # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") - # logger_mp.debug(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.""" - logger_mp.info("[H1_2_ArmController] ctrl_dual_arm_go_home start...") - max_attempts = 100 - current_attempts = 0 - 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 current_attempts < max_attempts: - current_q = self.get_current_dual_arm_q() - if np.all(np.abs(current_q) < tolerance): - logger_mp.info("[H1_2_ArmController] both arms have reached the home position.") - break - current_attempts += 1 - 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 - - -class H1_ArmController: - def __init__(self, simulation_mode=False): - self.simulation_mode = simulation_mode - - logger_mp.info("Initialize H1_ArmController...") - self.q_target = np.zeros(8) - self.tauff_target = np.zeros(8) - - self.kp_high = 300.0 - self.kd_high = 5.0 - self.kp_low = 140.0 - self.kd_low = 3.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 - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, go_LowCmd) - self.lowcmd_publisher.Init() - self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, go_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.1) - logger_mp.warning("[H1_ArmController] Waiting to subscribe dds...") - logger_mp.info("[H1_ArmController] Subscribe dds ok.") - - # initialize h1's lowcmd msg - self.crc = CRC() - self.msg = unitree_go_msg_dds__LowCmd_() - self.msg.head[0] = 0xFE - self.msg.head[1] = 0xEF - self.msg.level_flag = 0xFF - self.msg.gpio = 0 - - self.all_motor_q = self.get_current_motor_q() - logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") - logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") - - for id in H1_JointIndex: - if self._Is_weak_motor(id): - self.msg.motor_cmd[id].kp = self.kp_low - self.msg.motor_cmd[id].kd = self.kd_low - self.msg.motor_cmd[id].mode = 0x01 - else: - self.msg.motor_cmd[id].kp = self.kp_high - self.msg.motor_cmd[id].kd = self.kd_high - self.msg.motor_cmd[id].mode = 0x0A - self.msg.motor_cmd[id].q = self.all_motor_q[id] - logger_mp.info("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() - - logger_mp.info("Initialize H1_ArmController OK!\n") - - def _subscribe_motor_state(self): - while True: - msg = self.lowstate_subscriber.Read() - if msg is not None: - lowstate = H1_LowState() - for id in range(H1_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 - - if self.simulation_mode: - cliped_arm_q_target = arm_q_target - else: - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit=self.arm_velocity_limit) - - for idx, id in enumerate(H1_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) - # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") - # logger_mp.debug(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_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_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_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_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.""" - logger_mp.info("[H1_ArmController] ctrl_dual_arm_go_home start...") - max_attempts = 100 - current_attempts = 0 - with self.ctrl_lock: - self.q_target = np.zeros(8) - # self.tauff_target = np.zeros(8) - tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements - while current_attempts < max_attempts: - current_q = self.get_current_dual_arm_q() - if np.all(np.abs(current_q) < tolerance): - logger_mp.info("[H1_ArmController] both arms have reached the home position.") - break - current_attempts += 1 - 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_JointIndex.kLeftAnkle.value, - H1_JointIndex.kRightAnkle.value, - # Left arm - H1_JointIndex.kLeftShoulderPitch.value, - H1_JointIndex.kLeftShoulderRoll.value, - H1_JointIndex.kLeftShoulderYaw.value, - H1_JointIndex.kLeftElbow.value, - # Right arm - H1_JointIndex.kRightShoulderPitch.value, - H1_JointIndex.kRightShoulderRoll.value, - H1_JointIndex.kRightShoulderYaw.value, - H1_JointIndex.kRightElbow.value, - ] - return motor_index.value in weak_motors - - -class H1_JointArmIndex(IntEnum): - # Unlike G1 and H1_2, the arm order in DDS messages for H1 is right then left. - # Therefore, the purpose of switching the order here is to maintain consistency with G1 and H1_2. - # Left arm - kLeftShoulderPitch = 16 - kLeftShoulderRoll = 17 - kLeftShoulderYaw = 18 - kLeftElbow = 19 - # Right arm - kRightShoulderPitch = 12 - kRightShoulderRoll = 13 - kRightShoulderYaw = 14 - kRightElbow = 15 - - -class H1_JointIndex(IntEnum): - kRightHipRoll = 0 - kRightHipPitch = 1 - kRightKnee = 2 - kLeftHipRoll = 3 - kLeftHipPitch = 4 - kLeftKnee = 5 - kWaistYaw = 6 - kLeftHipYaw = 7 - kRightHipYaw = 8 - kNotUsedJoint = 9 - kLeftAnkle = 10 - kRightAnkle = 11 - # Right arm - kRightShoulderPitch = 12 - kRightShoulderRoll = 13 - kRightShoulderYaw = 14 - kRightElbow = 15 - # Left arm - kLeftShoulderPitch = 16 - kLeftShoulderRoll = 17 - kLeftShoulderYaw = 18 - kLeftElbow = 19 - - -if __name__ == "__main__": - from robot_arm_ik import G1_29_ArmIK - import pinocchio as pin - - arm_ik = G1_29_ArmIK(Unit_Test=True, Visualization=False) - arm = G1_29_ArmController(simulation_mode=True) - # arm_ik = G1_23_ArmIK(Unit_Test = True, Visualization = False) - # arm = G1_23_ArmController() - # arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = False) - # arm = H1_2_ArmController() - # arm_ik = H1_ArmIK(Unit_Test = True, Visualization = True) - # arm = H1_ArmController() - - # initial positon - L_tf_target = pin.SE3( - pin.Quaternion(1, 0, 0, 0), - np.array([0.25, +0.25, 0.1]), - ) - - R_tf_target = pin.SE3( - pin.Quaternion(1, 0, 0, 0), - np.array([0.25, -0.25, 0.1]), - ) - - rotation_speed = 0.005 # Rotation speed in radians per iteration - - user_input = input("Please enter the start signal (enter 's' to start the subsequent program): \n") - if user_input.lower() == "s": - step = 0 - arm.speed_gradual_max() - while True: - if step <= 120: - angle = rotation_speed * step - L_quat = pin.Quaternion(np.cos(angle / 2), 0, np.sin(angle / 2), 0) # y axis - R_quat = pin.Quaternion(np.cos(angle / 2), 0, 0, np.sin(angle / 2)) # z axis - - L_tf_target.translation += np.array([0.001, 0.001, 0.001]) - R_tf_target.translation += np.array([0.001, -0.001, 0.001]) - else: - angle = rotation_speed * (240 - step) - L_quat = pin.Quaternion(np.cos(angle / 2), 0, np.sin(angle / 2), 0) # y axis - R_quat = pin.Quaternion(np.cos(angle / 2), 0, 0, np.sin(angle / 2)) # z axis - - L_tf_target.translation -= np.array([0.001, 0.001, 0.001]) - R_tf_target.translation -= np.array([0.001, -0.001, 0.001]) - - L_tf_target.rotation = L_quat.toRotationMatrix() - R_tf_target.rotation = R_quat.toRotationMatrix() - - 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 - ) - - arm.ctrl_dual_arm(sol_q, sol_tauff) - - step += 1 - if step > 240: - step = 0 - time.sleep(0.01) diff --git a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm_ik.py b/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm_ik.py deleted file mode 100644 index 8392fe12f..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm_ik.py +++ /dev/null @@ -1,1148 +0,0 @@ -import casadi -import meshcat.geometry as mg -import numpy as np -import pinocchio as pin -import time -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 - -logger_mp = logging_mp.get_logger(__name__) -parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -sys.path.append(parent2_dir) - - -class G1_29_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( - "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/eval_robot/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/" - ) # for test - - self.mixed_jointsToLockIDs = [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_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) - # logger_mp.debug(f"Frame ID: {frame_id}, Name: {frame.name}") - # for idx, name in enumerate(self.reduced_robot.model.names): - # logger_mp.debug(f"{idx}: {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=[107, 108], 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 = 20 - 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: - logger_mp.error(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) - ) - - logger_mp.error( - 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) - - def solve_tau(self, current_lr_arm_motor_q=None, current_lr_arm_motor_dq=None): - try: - sol_tauff = pin.rnea( - self.reduced_robot.model, - self.reduced_robot.data, - current_lr_arm_motor_q, - np.zeros(14), - np.zeros(self.reduced_robot.model.nv), - ) - return sol_tauff - - except Exception as e: - logger_mp.error(f"ERROR in convergence, plotting debug info.{e}") - return np.zeros(self.reduced_robot.model.nv) - - -class G1_23_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( - "unitree_lerobot/eval_robot/assets/g1/g1_body23.urdf", "unitree_lerobot/eval_robot/assets/g1/" - ) - else: - self.robot = pin.RobotWrapper.BuildFromURDF( - "unitree_lerobot/eval_robot/assets/g1/g1_body23.urdf", "unitree_lerobot/eval_robot/assets/g1/" - ) # for test - - self.mixed_jointsToLockIDs = [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_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_roll_joint"), - pin.SE3(np.eye(3), np.array([0.20, 0, 0]).T), - pin.FrameType.OP_FRAME, - ) - ) - - self.reduced_robot.model.addFrame( - pin.Frame( - "R_ee", - self.reduced_robot.model.getJointId("right_wrist_roll_joint"), - pin.SE3(np.eye(3), np.array([0.20, 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) - # logger_mp.debug(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 - + 0.5 * 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]), 10) - 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=[67, 68], 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 = 20 - 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: - logger_mp.error(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) - ) - - logger_mp.error( - 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) - - def solve_tau(self, current_lr_arm_motor_q=None, current_lr_arm_motor_dq=None): - try: - sol_tauff = pin.rnea( - self.reduced_robot.model, - self.reduced_robot.data, - current_lr_arm_motor_q, - np.zeros(14), - np.zeros(self.reduced_robot.model.nv), - ) - return sol_tauff - - except Exception as e: - logger_mp.error(f"ERROR in convergence, plotting debug info.{e}") - return 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( - "unitree_lerobot/eval_robot/assets/h1_2/h1_2.urdf", "unitree_lerobot/eval_robot/assets/h1_2/" - ) - else: - self.robot = pin.RobotWrapper.BuildFromURDF( - "unitree_lerobot/eval_robot/assets/h1_2/h1_2.urdf", "unitree_lerobot/eval_robot/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) - # logger_mp.debug(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=[113, 114], 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: - logger_mp.error(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) - ) - - logger_mp.error( - 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) - - -class H1_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/h1_with_hand.urdf", "../assets/h1/") - else: - self.robot = pin.RobotWrapper.BuildFromURDF( - "../../assets/h1/h1_with_hand.urdf", "../../assets/h1/" - ) # for test - - self.mixed_jointsToLockIDs = [ - "right_hip_roll_joint", - "right_hip_pitch_joint", - "right_knee_joint", - "left_hip_roll_joint", - "left_hip_pitch_joint", - "left_knee_joint", - "torso_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_ankle_joint", - "right_ankle_joint", - "L_index_proximal_joint", - "L_index_intermediate_joint", - "L_middle_proximal_joint", - "L_middle_intermediate_joint", - "L_ring_proximal_joint", - "L_ring_intermediate_joint", - "L_pinky_proximal_joint", - "L_pinky_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_ring_proximal_joint", - "R_ring_intermediate_joint", - "R_pinky_proximal_joint", - "R_pinky_intermediate_joint", - "R_thumb_proximal_yaw_joint", - "R_thumb_proximal_pitch_joint", - "R_thumb_intermediate_joint", - "R_thumb_distal_joint", - "left_hand_joint", - "right_hand_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_elbow_joint"), - pin.SE3(np.eye(3), np.array([0.2605 + 0.05, 0, 0]).T), - pin.FrameType.OP_FRAME, - ) - ) - - self.reduced_robot.model.addFrame( - pin.Frame( - "R_ee", - self.reduced_robot.model.getJointId("right_elbow_joint"), - pin.SE3(np.eye(3), np.array([0.2605 + 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) - # logger_mp.debug(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 - + 0.5 * 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]), 8) - 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=[105, 106], 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.3, 0.3], - [1.0, 0.7, 0.7], - [0.3, 1.0, 0.5], - [0.7, 1.0, 0.8], - [0.3, 0.8, 1.0], - [0.7, 0.9, 1.0], - ] - ) - .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: - logger_mp.error(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) - ) - - logger_mp.error( - 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 = H1_2_ArmIK(Unit_Test = True, Visualization = True) - # arm_ik = G1_23_ArmIK(Unit_Test = True, Visualization = True) - # arm_ik = H1_ArmIK(Unit_Test = True, Visualization = True) - - # initial positon - L_tf_target = pin.SE3( - pin.Quaternion(1, 0, 0, 0), - np.array([0.25, +0.25, 0.1]), - ) - - R_tf_target = pin.SE3( - pin.Quaternion(1, 0, 0, 0), - np.array([0.25, -0.25, 0.1]), - ) - - rotation_speed = 0.005 - noise_amplitude_translation = 0.001 - noise_amplitude_rotation = 0.01 - - user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n") - if user_input.lower() == "s": - step = 0 - while True: - # Apply rotation noise with bias towards y and z axes - rotation_noise_L = pin.Quaternion( - np.cos(np.random.normal(0, noise_amplitude_rotation) / 2), - 0, - np.random.normal(0, noise_amplitude_rotation / 2), - 0, - ).normalized() # y bias - - rotation_noise_R = pin.Quaternion( - np.cos(np.random.normal(0, noise_amplitude_rotation) / 2), - 0, - 0, - np.random.normal(0, noise_amplitude_rotation / 2), - ).normalized() # z bias - - if step <= 120: - angle = rotation_speed * step - L_tf_target.rotation = ( - rotation_noise_L * pin.Quaternion(np.cos(angle / 2), 0, np.sin(angle / 2), 0) - ).toRotationMatrix() # y axis - R_tf_target.rotation = ( - rotation_noise_R * pin.Quaternion(np.cos(angle / 2), 0, 0, np.sin(angle / 2)) - ).toRotationMatrix() # z axis - L_tf_target.translation += np.array([0.001, 0.001, 0.001]) + np.random.normal( - 0, noise_amplitude_translation, 3 - ) - R_tf_target.translation += np.array([0.001, -0.001, 0.001]) + np.random.normal( - 0, noise_amplitude_translation, 3 - ) - else: - angle = rotation_speed * (240 - step) - L_tf_target.rotation = ( - rotation_noise_L * pin.Quaternion(np.cos(angle / 2), 0, np.sin(angle / 2), 0) - ).toRotationMatrix() # y axis - R_tf_target.rotation = ( - rotation_noise_R * pin.Quaternion(np.cos(angle / 2), 0, 0, np.sin(angle / 2)) - ).toRotationMatrix() # z axis - L_tf_target.translation -= np.array([0.001, 0.001, 0.001]) + np.random.normal( - 0, noise_amplitude_translation, 3 - ) - R_tf_target.translation -= np.array([0.001, -0.001, 0.001]) + np.random.normal( - 0, noise_amplitude_translation, 3 - ) - - arm_ik.solve_ik(L_tf_target.homogeneous, R_tf_target.homogeneous) - - step += 1 - if step > 240: - step = 0 - time.sleep(0.1) diff --git a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm_test.py b/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm_test.py deleted file mode 100644 index cfbcf3ca6..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_arm_test.py +++ /dev/null @@ -1,343 +0,0 @@ -import numpy as np -import threading -import time -from enum import IntEnum - -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds -from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as hg_LowCmd, LowState_ as hg_LowState # idl for g1, h1_2 -from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ -from unitree_sdk2py.utils.crc import CRC - -from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as go_LowCmd, LowState_ as go_LowState # idl for h1 -from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ - -import logging_mp - -logger_mp = logging_mp.get_logger(__name__) - -kTopicLowCommand_Debug = "rt/lowcmd" -kTopicLowCommand_Motion = "rt/arm_sdk" -kTopicLowState = "rt/lowstate" - -G1_29_Num_Motors = 35 -G1_23_Num_Motors = 35 -H1_2_Num_Motors = 35 -H1_Num_Motors = 20 - - -class MotorState: - def __init__(self): - self.q = None - self.dq = None - - -class G1_29_LowState: - def __init__(self): - self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)] - -class DataBuffer: - def __init__(self): - self.data = None - self.lock = threading.Lock() - - def GetData(self): - with self.lock: - return self.data - - def SetData(self, data): - with self.lock: - self.data = data - - -class G1_29_ArmController: - def __init__(self, motion_mode=False, simulation_mode=False): - logger_mp.info("Initialize G1_29_ArmController...") - self.q_target = np.zeros(14) - self.tauff_target = np.zeros(14) - self.motion_mode = motion_mode - self.simulation_mode = simulation_mode - self.kp_high = 40.0 - self.kd_high = 3.0 - self.kp_low = 80.0 - self.kd_low = 3.0 - self.kp_wrist = 40.0 - self.kd_wrist = 1.5 - - self.all_motor_q = None - self.arm_velocity_limit = 100.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 - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - if self.motion_mode: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd) - else: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) - self.lowcmd_publisher.Init() - self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_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.1) - logger_mp.warning("[G1_29_ArmController] Waiting to subscribe dds...") - logger_mp.info("[G1_29_ArmController] Subscribe dds ok.") - - # 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() - print(self.msg) - - self.all_motor_q = self.get_current_motor_q() - logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") - logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") - - arm_indices = set(member.value for member in G1_29_JointArmIndex) - for id in G1_29_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 current motor q, kp, kd - - logger_mp.info("Lock OK!\n") #motors are not locked x - # for i in range(10000): - # print(self.get_current_motor_q()) - # time.sleep(0.05) - # 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() - - logger_mp.info("Initialize G1_29_ArmController OK!\n") - - def _subscribe_motor_state(self): - while True: - msg = self.lowstate_subscriber.Read() - if msg is not None: - lowstate = G1_29_LowState() - for id in range(G1_29_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) - - 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): - if self.motion_mode: - self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0 - - while True: - start_time = time.time() - with self.ctrl_lock: - arm_q_target = self.q_target - arm_tauff_target = self.tauff_target - - if self.simulation_mode: - cliped_arm_q_target = arm_q_target - else: - 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) - # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") - # logger_mp.debug(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 G1_29_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 G1_29_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 G1_29_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.""" - logger_mp.info("[G1_29_ArmController] ctrl_dual_arm_go_home start...") - max_attempts = 100 - current_attempts = 0 - with self.ctrl_lock: - self.q_target = np.zeros(14) - #self.q_target[G1_29_JointIndex.kLeftElbow] = 0.5 - # 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 current_attempts < max_attempts: - current_q = self.get_current_dual_arm_q() - if np.all(np.abs(current_q) < tolerance): - if self.motion_mode: - for weight in np.linspace(1, 0, num=101): - self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight - time.sleep(0.02) - logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") - break - current_attempts += 1 - 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 = [ - G1_29_JointIndex.kLeftAnklePitch.value, - G1_29_JointIndex.kRightAnklePitch.value, - # Left arm - G1_29_JointIndex.kLeftShoulderPitch.value, - G1_29_JointIndex.kLeftShoulderRoll.value, - G1_29_JointIndex.kLeftShoulderYaw.value, - G1_29_JointIndex.kLeftElbow.value, - # Right arm - G1_29_JointIndex.kRightShoulderPitch.value, - G1_29_JointIndex.kRightShoulderRoll.value, - G1_29_JointIndex.kRightShoulderYaw.value, - G1_29_JointIndex.kRightElbow.value, - ] - return motor_index.value in weak_motors - - def _Is_wrist_motor(self, motor_index): - wrist_motors = [ - G1_29_JointIndex.kLeftWristRoll.value, - G1_29_JointIndex.kLeftWristPitch.value, - G1_29_JointIndex.kLeftWristyaw.value, - G1_29_JointIndex.kRightWristRoll.value, - G1_29_JointIndex.kRightWristPitch.value, - G1_29_JointIndex.kRightWristYaw.value, - ] - return motor_index.value in wrist_motors - - -class G1_29_JointArmIndex(IntEnum): - # Left arm - kLeftShoulderPitch = 15 - kLeftShoulderRoll = 16 - kLeftShoulderYaw = 17 - kLeftElbow = 18 - kLeftWristRoll = 19 - kLeftWristPitch = 20 - kLeftWristyaw = 21 - - # Right arm - kRightShoulderPitch = 22 - kRightShoulderRoll = 23 - kRightShoulderYaw = 24 - kRightElbow = 25 - kRightWristRoll = 26 - kRightWristPitch = 27 - kRightWristYaw = 28 - - -class G1_29_JointIndex(IntEnum): - # Left leg - kLeftHipPitch = 0 - kLeftHipRoll = 1 - kLeftHipYaw = 2 - kLeftKnee = 3 - kLeftAnklePitch = 4 - kLeftAnkleRoll = 5 - - # Right leg - kRightHipPitch = 6 - kRightHipRoll = 7 - kRightHipYaw = 8 - kRightKnee = 9 - kRightAnklePitch = 10 - kRightAnkleRoll = 11 - - kWaistYaw = 12 #we're c - kWaistRoll = 13 - kWaistPitch = 14 - - # Left arm - kLeftShoulderPitch = 15 - kLeftShoulderRoll = 16 - kLeftShoulderYaw = 17 - kLeftElbow = 18 - kLeftWristRoll = 19 - kLeftWristPitch = 20 - kLeftWristyaw = 21 - - # Right arm - kRightShoulderPitch = 22 - kRightShoulderRoll = 23 - kRightShoulderYaw = 24 - kRightElbow = 25 - kRightWristRoll = 26 - kRightWristPitch = 27 - kRightWristYaw = 28 - - # not used - kNotUsedJoint0 = 29 - kNotUsedJoint1 = 30 - kNotUsedJoint2 = 31 - kNotUsedJoint3 = 32 - kNotUsedJoint4 = 33 - kNotUsedJoint5 = 34 - diff --git a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_brainco.py b/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_brainco.py deleted file mode 100644 index f95d28b60..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_brainco.py +++ /dev/null @@ -1,196 +0,0 @@ -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds -from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ # idl -from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_ - -import numpy as np -from enum import IntEnum -import threading -import time -from multiprocessing import Process, Array - -import logging_mp - -logger_mp = logging_mp.get_logger(__name__) - -brainco_Num_Motors = 6 -kTopicbraincoLeftCommand = "rt/brainco/left/cmd" -kTopicbraincoLeftState = "rt/brainco/left/state" -kTopicbraincoRightCommand = "rt/brainco/right/cmd" -kTopicbraincoRightState = "rt/brainco/right/state" - - -class Brainco_Controller: - def __init__( - self, - left_hand_array, - right_hand_array, - dual_hand_data_lock=None, - dual_hand_state_array=None, - dual_hand_action_array=None, - fps=100.0, - Unit_Test=False, - simulation_mode=False, - ): - logger_mp.info("Initialize Brainco_Controller...") - self.fps = fps - self.hand_sub_ready = False - self.Unit_Test = Unit_Test - self.simulation_mode = simulation_mode - - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - # initialize handcmd publisher and handstate subscriber - self.LeftHandCmb_publisher = ChannelPublisher(kTopicbraincoLeftCommand, MotorCmds_) - self.LeftHandCmb_publisher.Init() - self.RightHandCmb_publisher = ChannelPublisher(kTopicbraincoRightCommand, MotorCmds_) - self.RightHandCmb_publisher.Init() - - self.LeftHandState_subscriber = ChannelSubscriber(kTopicbraincoLeftState, MotorStates_) - self.LeftHandState_subscriber.Init() - self.RightHandState_subscriber = ChannelSubscriber(kTopicbraincoRightState, MotorStates_) - self.RightHandState_subscriber.Init() - - # Shared Arrays for hand states - self.left_hand_state_array = Array("d", brainco_Num_Motors, lock=True) - self.right_hand_state_array = Array("d", brainco_Num_Motors, lock=True) - - # initialize subscribe thread - self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state) - self.subscribe_state_thread.daemon = True - self.subscribe_state_thread.start() - - while not self.hand_sub_ready: - time.sleep(0.1) - logger_mp.warning("[brainco_Controller] Waiting to subscribe dds...") - logger_mp.info("[brainco_Controller] Subscribe dds ok.") - - hand_control_process = Process( - target=self.control_process, - args=( - left_hand_array, - right_hand_array, - self.left_hand_state_array, - self.right_hand_state_array, - dual_hand_data_lock, - dual_hand_state_array, - dual_hand_action_array, - ), - ) - hand_control_process.daemon = True - hand_control_process.start() - - logger_mp.info("Initialize brainco_Controller OK!\n") - - def _subscribe_hand_state(self): - while True: - left_hand_msg = self.LeftHandState_subscriber.Read() - right_hand_msg = self.RightHandState_subscriber.Read() - self.hand_sub_ready = True - if left_hand_msg is not None and right_hand_msg is not None: - # Update left hand state - for idx, id in enumerate(Brainco_Left_Hand_JointIndex): - self.left_hand_state_array[idx] = left_hand_msg.states[id].q - # Update right hand state - for idx, id in enumerate(Brainco_Right_Hand_JointIndex): - self.right_hand_state_array[idx] = right_hand_msg.states[id].q - time.sleep(0.002) - - def ctrl_dual_hand(self, left_q_target, right_q_target): - """ - Set current left, right hand motor state target q - """ - for idx, id in enumerate(Brainco_Left_Hand_JointIndex): - self.left_hand_msg.cmds[id].q = left_q_target[idx] - for idx, id in enumerate(Brainco_Right_Hand_JointIndex): - self.right_hand_msg.cmds[id].q = right_q_target[idx] - - self.LeftHandCmb_publisher.Write(self.left_hand_msg) - self.RightHandCmb_publisher.Write(self.right_hand_msg) - # logger_mp.debug("hand ctrl publish ok.") - - def control_process( - self, - left_hand_array, - right_hand_array, - left_hand_state_array, - right_hand_state_array, - dual_hand_data_lock=None, - dual_hand_state_array=None, - dual_hand_action_array=None, - ): - self.running = True - - left_q_target = np.full(brainco_Num_Motors, 0) - right_q_target = np.full(brainco_Num_Motors, 0) - - # initialize brainco hand's cmd msg - self.left_hand_msg = MotorCmds_() - self.left_hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(len(Brainco_Left_Hand_JointIndex))] - self.right_hand_msg = MotorCmds_() - self.right_hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(len(Brainco_Right_Hand_JointIndex))] - - for idx, id in enumerate(Brainco_Left_Hand_JointIndex): - self.left_hand_msg.cmds[id].q = 0.0 - self.left_hand_msg.cmds[id].dq = 1.0 - for idx, id in enumerate(Brainco_Right_Hand_JointIndex): - self.right_hand_msg.cmds[id].q = 0.0 - self.right_hand_msg.cmds[id].dq = 1.0 - - try: - while self.running: - start_time = time.time() - # get dual hand state - with left_hand_array.get_lock(): - left_hand_mat = np.array(left_hand_array[:]).copy() - with right_hand_array.get_lock(): - right_hand_mat = np.array(right_hand_array[:]).copy() - - # Read left and right q_state from shared arrays - state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) - - action_data = np.concatenate((left_hand_mat, right_hand_mat)) - if dual_hand_data_lock is not None: - with dual_hand_data_lock: - dual_hand_state_array[:] = state_data - dual_hand_action_array[:] = action_data - - if dual_hand_state_array and dual_hand_action_array: - with dual_hand_data_lock: - left_q_target = left_hand_mat - right_q_target = right_hand_mat - - self.ctrl_dual_hand(left_q_target, right_q_target) - current_time = time.time() - time_elapsed = current_time - start_time - sleep_time = max(0, (1 / self.fps) - time_elapsed) - time.sleep(sleep_time) - finally: - logger_mp.info("brainco_Controller has been closed.") - - -# according to the official documentation, https://www.brainco-hz.com/docs/revolimb-hand/product/parameters.html -# the motor sequence is as shown in the table below -# ┌──────┬───────┬────────────┬────────┬────────┬────────┬────────┐ -# │ Id │ 0 │ 1 │ 2 │ 3 │ 4 │ 5 │ -# ├──────┼───────┼────────────┼────────┼────────┼────────┼────────┤ -# │Joint │ thumb │ thumb-aux | index │ middle │ ring │ pinky │ -# └──────┴───────┴────────────┴────────┴────────┴────────┴────────┘ -class Brainco_Right_Hand_JointIndex(IntEnum): - kRightHandThumb = 0 - kRightHandThumbAux = 1 - kRightHandIndex = 2 - kRightHandMiddle = 3 - kRightHandRing = 4 - kRightHandPinky = 5 - - -class Brainco_Left_Hand_JointIndex(IntEnum): - kLeftHandThumb = 0 - kLeftHandThumbAux = 1 - kLeftHandIndex = 2 - kLeftHandMiddle = 3 - kLeftHandRing = 4 - kLeftHandPinky = 5 diff --git a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_inspire.py b/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_inspire.py deleted file mode 100644 index dcc971140..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_inspire.py +++ /dev/null @@ -1,187 +0,0 @@ -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds -from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ # idl -from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_ - -import numpy as np -from enum import IntEnum -import threading -import time -from multiprocessing import Process, Array - -import logging_mp - -logger_mp = logging_mp.get_logger(__name__) - -Inspire_Num_Motors = 6 -kTopicInspireCommand = "rt/inspire/cmd" -kTopicInspireState = "rt/inspire/state" - - -class Inspire_Controller: - def __init__( - self, - left_hand_array, - right_hand_array, - dual_hand_data_lock=None, - dual_hand_state_array=None, - dual_hand_action_array=None, - fps=100.0, - Unit_Test=False, - simulation_mode=False, - ): - logger_mp.info("Initialize Inspire_Controller...") - self.fps = fps - self.Unit_Test = Unit_Test - self.simulation_mode = simulation_mode - - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - # initialize handcmd publisher and handstate subscriber - self.HandCmb_publisher = ChannelPublisher(kTopicInspireCommand, MotorCmds_) - self.HandCmb_publisher.Init() - - self.HandState_subscriber = ChannelSubscriber(kTopicInspireState, MotorStates_) - self.HandState_subscriber.Init() - - # Shared Arrays for hand states - self.left_hand_state_array = Array("d", Inspire_Num_Motors, lock=True) - self.right_hand_state_array = Array("d", Inspire_Num_Motors, lock=True) - - # initialize subscribe thread - self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state) - self.subscribe_state_thread.daemon = True - self.subscribe_state_thread.start() - - while True: - if any(self.right_hand_state_array): # any(self.left_hand_state_array) and - break - time.sleep(0.01) - logger_mp.warning("[Inspire_Controller] Waiting to subscribe dds...") - logger_mp.info("[Inspire_Controller] Subscribe dds ok.") - - hand_control_process = Process( - target=self.control_process, - args=( - left_hand_array, - right_hand_array, - self.left_hand_state_array, - self.right_hand_state_array, - dual_hand_data_lock, - dual_hand_state_array, - dual_hand_action_array, - ), - ) - hand_control_process.daemon = True - hand_control_process.start() - - logger_mp.info("Initialize Inspire_Controller OK!\n") - - def _subscribe_hand_state(self): - while True: - hand_msg = self.HandState_subscriber.Read() - if hand_msg is not None: - for idx, id in enumerate(Inspire_Left_Hand_JointIndex): - self.left_hand_state_array[idx] = hand_msg.states[id].q - for idx, id in enumerate(Inspire_Right_Hand_JointIndex): - self.right_hand_state_array[idx] = hand_msg.states[id].q - time.sleep(0.002) - - def ctrl_dual_hand(self, left_q_target, right_q_target): - """ - Set current left, right hand motor state target q - """ - for idx, id in enumerate(Inspire_Left_Hand_JointIndex): - self.hand_msg.cmds[id].q = left_q_target[idx] - for idx, id in enumerate(Inspire_Right_Hand_JointIndex): - self.hand_msg.cmds[id].q = right_q_target[idx] - - self.HandCmb_publisher.Write(self.hand_msg) - # logger_mp.debug("hand ctrl publish ok.") - - def control_process( - self, - left_hand_array, - right_hand_array, - left_hand_state_array, - right_hand_state_array, - dual_hand_data_lock=None, - dual_hand_state_array=None, - dual_hand_action_array=None, - ): - self.running = True - - left_q_target = np.full(Inspire_Num_Motors, 1.0) - right_q_target = np.full(Inspire_Num_Motors, 1.0) - - # initialize inspire hand's cmd msg - self.hand_msg = MotorCmds_() - self.hand_msg.cmds = [ - unitree_go_msg_dds__MotorCmd_() - for _ in range(len(Inspire_Right_Hand_JointIndex) + len(Inspire_Left_Hand_JointIndex)) - ] - - for idx, id in enumerate(Inspire_Left_Hand_JointIndex): - self.hand_msg.cmds[id].q = 1.0 - for idx, id in enumerate(Inspire_Right_Hand_JointIndex): - self.hand_msg.cmds[id].q = 1.0 - - try: - while self.running: - start_time = time.time() - - # get dual hand state - with left_hand_array.get_lock(): - left_hand_mat = np.array(left_hand_array[:]).copy() - with right_hand_array.get_lock(): - right_hand_mat = np.array(right_hand_array[:]).copy() - - # Read left and right q_state from shared arrays - state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) - - action_data = np.concatenate((left_hand_mat, right_hand_mat)) - if dual_hand_data_lock is not None: - with dual_hand_data_lock: - dual_hand_state_array[:] = state_data - dual_hand_action_array[:] = action_data - - if dual_hand_state_array and dual_hand_action_array: - with dual_hand_data_lock: - left_q_target = left_hand_mat - right_q_target = right_hand_mat - - self.ctrl_dual_hand(left_q_target, right_q_target) - current_time = time.time() - time_elapsed = current_time - start_time - sleep_time = max(0, (1 / self.fps) - time_elapsed) - time.sleep(sleep_time) - finally: - logger_mp.info("Inspire_Controller has been closed.") - - -# Update hand state, according to the official documentation, https://support.unitree.com/home/en/G1_developer/inspire_dfx_dexterous_hand -# the state sequence is as shown in the table below -# ┌──────┬───────┬──────┬────────┬────────┬────────────┬────────────────┬───────┬──────┬────────┬────────┬────────────┬────────────────┐ -# │ Id │ 0 │ 1 │ 2 │ 3 │ 4 │ 5 │ 6 │ 7 │ 8 │ 9 │ 10 │ 11 │ -# ├──────┼───────┼──────┼────────┼────────┼────────────┼────────────────┼───────┼──────┼────────┼────────┼────────────┼────────────────┤ -# │ │ Right Hand │ Left Hand │ -# │Joint │ pinky │ ring │ middle │ index │ thumb-bend │ thumb-rotation │ pinky │ ring │ middle │ index │ thumb-bend │ thumb-rotation │ -# └──────┴───────┴──────┴────────┴────────┴────────────┴────────────────┴───────┴──────┴────────┴────────┴────────────┴────────────────┘ -class Inspire_Right_Hand_JointIndex(IntEnum): - kRightHandPinky = 0 - kRightHandRing = 1 - kRightHandMiddle = 2 - kRightHandIndex = 3 - kRightHandThumbBend = 4 - kRightHandThumbRotation = 5 - - -class Inspire_Left_Hand_JointIndex(IntEnum): - kLeftHandPinky = 6 - kLeftHandRing = 7 - kLeftHandMiddle = 8 - kLeftHandIndex = 9 - kLeftHandThumbBend = 10 - kLeftHandThumbRotation = 11 diff --git a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_unitree.py b/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_unitree.py deleted file mode 100644 index 013c43c34..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/robot_control/robot_hand_unitree.py +++ /dev/null @@ -1,403 +0,0 @@ -# for dex3-1 -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds -from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_ # idl -from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_ - -# for gripper -from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ # idl -from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_ - -import numpy as np -from enum import IntEnum -import time -import threading -from multiprocessing import Process, Value, Array - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -unitree_tip_indices = [4, 9, 14] # [thumb, index, middle] in OpenXR -Dex3_Num_Motors = 7 -kTopicDex3LeftCommand = "rt/dex3/left/cmd" -kTopicDex3RightCommand = "rt/dex3/right/cmd" -kTopicDex3LeftState = "rt/dex3/left/state" -kTopicDex3RightState = "rt/dex3/right/state" - - -class Dex3_1_Controller: - def __init__( - self, - left_hand_array_in, - right_hand_array_in, - dual_hand_data_lock=None, - dual_hand_state_array_out=None, - dual_hand_action_array_out=None, - fps=100.0, - Unit_Test=False, - simulation_mode=False, - ): - """ - [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process - left_hand_array_in: [input] Left hand skeleton data (required from XR device) to hand_ctrl.control_process - right_hand_array_in: [input] Right hand skeleton data (required from XR device) to hand_ctrl.control_process - dual_hand_data_lock: Data synchronization lock for dual_hand_state_array and dual_hand_action_array - dual_hand_state_array_out: [output] Return left(7), right(7) hand motor state - dual_hand_action_array_out: [output] Return left(7), right(7) hand motor action - fps: Control frequency - Unit_Test: Whether to enable unit testing - simulation_mode: Whether to use simulation mode (default is False, which means using real robot) - """ - logger_mp.info("Initialize Dex3_1_Controller...") - - self.fps = fps - self.Unit_Test = Unit_Test - self.simulation_mode = simulation_mode - - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - # initialize handcmd publisher and handstate subscriber - self.LeftHandCmb_publisher = ChannelPublisher(kTopicDex3LeftCommand, HandCmd_) - self.LeftHandCmb_publisher.Init() - self.RightHandCmb_publisher = ChannelPublisher(kTopicDex3RightCommand, HandCmd_) - self.RightHandCmb_publisher.Init() - - self.LeftHandState_subscriber = ChannelSubscriber(kTopicDex3LeftState, HandState_) - self.LeftHandState_subscriber.Init() - self.RightHandState_subscriber = ChannelSubscriber(kTopicDex3RightState, HandState_) - self.RightHandState_subscriber.Init() - - # Shared Arrays for hand states - self.left_hand_state_array = Array("d", Dex3_Num_Motors, lock=True) - self.right_hand_state_array = Array("d", Dex3_Num_Motors, lock=True) - - # initialize subscribe thread - self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state) - self.subscribe_state_thread.daemon = True - self.subscribe_state_thread.start() - - while True: - if any(self.left_hand_state_array) and any(self.right_hand_state_array): - break - time.sleep(0.01) - logger_mp.warning("[Dex3_1_Controller] Waiting to subscribe dds...") - logger_mp.info("[Dex3_1_Controller] Subscribe dds ok.") - - hand_control_process = Process( - target=self.control_process, - args=( - left_hand_array_in, - right_hand_array_in, - self.left_hand_state_array, - self.right_hand_state_array, - dual_hand_data_lock, - dual_hand_state_array_out, - dual_hand_action_array_out, - ), - ) - hand_control_process.daemon = True - hand_control_process.start() - - logger_mp.info("Initialize Dex3_1_Controller OK!\n") - - def _subscribe_hand_state(self): - while True: - left_hand_msg = self.LeftHandState_subscriber.Read() - right_hand_msg = self.RightHandState_subscriber.Read() - if left_hand_msg is not None and right_hand_msg is not None: - # Update left hand state - for idx, id in enumerate(Dex3_1_Left_JointIndex): - self.left_hand_state_array[idx] = left_hand_msg.motor_state[id].q - # Update right hand state - for idx, id in enumerate(Dex3_1_Right_JointIndex): - self.right_hand_state_array[idx] = right_hand_msg.motor_state[id].q - time.sleep(0.002) - - class _RIS_Mode: - def __init__(self, id=0, status=0x01, timeout=0): - self.motor_mode = 0 - self.id = id & 0x0F # 4 bits for id - self.status = status & 0x07 # 3 bits for status - self.timeout = timeout & 0x01 # 1 bit for timeout - - def _mode_to_uint8(self): - self.motor_mode |= self.id & 0x0F - self.motor_mode |= (self.status & 0x07) << 4 - self.motor_mode |= (self.timeout & 0x01) << 7 - return self.motor_mode - - def ctrl_dual_hand(self, left_q_target, right_q_target): - """set current left, right hand motor state target q""" - for idx, id in enumerate(Dex3_1_Left_JointIndex): - self.left_msg.motor_cmd[id].q = left_q_target[idx] - for idx, id in enumerate(Dex3_1_Right_JointIndex): - self.right_msg.motor_cmd[id].q = right_q_target[idx] - - self.LeftHandCmb_publisher.Write(self.left_msg) - self.RightHandCmb_publisher.Write(self.right_msg) - # logger_mp.debug("hand ctrl publish ok.") - - def control_process( - self, - left_hand_array_in, - right_hand_array_in, - left_hand_state_array, - right_hand_state_array, - dual_hand_data_lock=None, - dual_hand_state_array_out=None, - dual_hand_action_array_out=None, - ): - self.running = True - - # left_q_target = np.full(Dex3_Num_Motors, 0) - # right_q_target = np.full(Dex3_Num_Motors, 0) - - q = 0.0 - dq = 0.0 - tau = 0.0 - kp = 1.5 - kd = 0.2 - - # initialize dex3-1's left hand cmd msg - self.left_msg = unitree_hg_msg_dds__HandCmd_() - for id in Dex3_1_Left_JointIndex: - ris_mode = self._RIS_Mode(id=id, status=0x01) - motor_mode = ris_mode._mode_to_uint8() - self.left_msg.motor_cmd[id].mode = motor_mode - self.left_msg.motor_cmd[id].q = q - self.left_msg.motor_cmd[id].dq = dq - self.left_msg.motor_cmd[id].tau = tau - self.left_msg.motor_cmd[id].kp = kp - self.left_msg.motor_cmd[id].kd = kd - - # initialize dex3-1's right hand cmd msg - self.right_msg = unitree_hg_msg_dds__HandCmd_() - for id in Dex3_1_Right_JointIndex: - ris_mode = self._RIS_Mode(id=id, status=0x01) - motor_mode = ris_mode._mode_to_uint8() - self.right_msg.motor_cmd[id].mode = motor_mode - self.right_msg.motor_cmd[id].q = q - self.right_msg.motor_cmd[id].dq = dq - self.right_msg.motor_cmd[id].tau = tau - self.right_msg.motor_cmd[id].kp = kp - self.right_msg.motor_cmd[id].kd = kd - - try: - while self.running: - start_time = time.time() - - # get dual hand state - with left_hand_array_in.get_lock(): - left_hand_mat = np.array(left_hand_array_in[:]).copy() - with right_hand_array_in.get_lock(): - right_hand_mat = np.array(right_hand_array_in[:]).copy() - - # Read left and right q_state from shared arrays - state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) - - # get dual hand action - action_data = np.concatenate((left_hand_mat, right_hand_mat)) - if dual_hand_state_array_out and dual_hand_action_array_out: - with dual_hand_data_lock: - dual_hand_state_array_out[:] = state_data - dual_hand_action_array_out[:] = action_data - - self.ctrl_dual_hand(left_hand_mat, right_hand_mat) - current_time = time.time() - time_elapsed = current_time - start_time - sleep_time = max(0, (1 / self.fps) - time_elapsed) - time.sleep(sleep_time) - finally: - print("Dex3_1_Controller has been closed.") - - -class Dex3_1_Left_JointIndex(IntEnum): - kLeftHandThumb0 = 0 - kLeftHandThumb1 = 1 - kLeftHandThumb2 = 2 - kLeftHandMiddle0 = 3 - kLeftHandMiddle1 = 4 - kLeftHandIndex0 = 5 - kLeftHandIndex1 = 6 - - -class Dex3_1_Right_JointIndex(IntEnum): - kRightHandThumb0 = 0 - kRightHandThumb1 = 1 - kRightHandThumb2 = 2 - kRightHandIndex0 = 3 - kRightHandIndex1 = 4 - kRightHandMiddle0 = 5 - kRightHandMiddle1 = 6 - - -kTopicGripperLeftCommand = "rt/dex1/left/cmd" -kTopicGripperLeftState = "rt/dex1/left/state" -kTopicGripperRightCommand = "rt/dex1/right/cmd" -kTopicGripperRightState = "rt/dex1/right/state" - - -class Dex1_1_Gripper_Controller: - def __init__( - self, - left_gripper_value_in, - right_gripper_value_in, - dual_gripper_data_lock=None, - dual_gripper_state_out=None, - dual_gripper_action_out=None, - filter=True, - fps=200.0, - Unit_Test=False, - simulation_mode=False, - ): - """ - [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process - left_gripper_value_in: [input] Left ctrl data (required from XR device) to control_thread - right_gripper_value_in: [input] Right ctrl data (required from XR device) to control_thread - dual_gripper_data_lock: Data synchronization lock for dual_gripper_state_array and dual_gripper_action_array - dual_gripper_state_out: [output] Return left(1), right(1) gripper motor state - dual_gripper_action_out: [output] Return left(1), right(1) gripper motor action - fps: Control frequency - Unit_Test: Whether to enable unit testing - simulation_mode: Whether to use simulation mode (default is False, which means using real robot) - """ - - logger_mp.info("Initialize Dex1_1_Gripper_Controller...") - - self.fps = fps - self.Unit_Test = Unit_Test - self.gripper_sub_ready = False - self.simulation_mode = simulation_mode - - if self.simulation_mode: - ChannelFactoryInitialize(1) - else: - ChannelFactoryInitialize(0) - - # initialize handcmd publisher and handstate subscriber - self.LeftGripperCmb_publisher = ChannelPublisher(kTopicGripperLeftCommand, MotorCmds_) - self.LeftGripperCmb_publisher.Init() - self.RightGripperCmb_publisher = ChannelPublisher(kTopicGripperRightCommand, MotorCmds_) - self.RightGripperCmb_publisher.Init() - - self.LeftGripperState_subscriber = ChannelSubscriber(kTopicGripperLeftState, MotorStates_) - self.LeftGripperState_subscriber.Init() - self.RightGripperState_subscriber = ChannelSubscriber(kTopicGripperRightState, MotorStates_) - self.RightGripperState_subscriber.Init() - - # Shared Arrays for gripper states - self.left_gripper_state_value = Value("d", 0.0, lock=True) - self.right_gripper_state_value = Value("d", 0.0, lock=True) - - # initialize subscribe thread - self.subscribe_state_thread = threading.Thread(target=self._subscribe_gripper_state) - self.subscribe_state_thread.daemon = True - self.subscribe_state_thread.start() - - while not self.gripper_sub_ready: - time.sleep(0.01) - logger_mp.warning("[Dex1_1_Gripper_Controller] Waiting to subscribe dds...") - logger_mp.info("[Dex1_1_Gripper_Controller] Subscribe dds ok.") - - self.gripper_control_thread = threading.Thread( - target=self.control_thread, - args=( - left_gripper_value_in, - right_gripper_value_in, - self.left_gripper_state_value, - self.right_gripper_state_value, - dual_gripper_data_lock, - dual_gripper_state_out, - dual_gripper_action_out, - ), - ) - self.gripper_control_thread.daemon = True - self.gripper_control_thread.start() - - logger_mp.info("Initialize Dex1_1_Gripper_Controller OK!\n") - - def _subscribe_gripper_state(self): - while True: - left_gripper_msg = self.LeftGripperState_subscriber.Read() - right_gripper_msg = self.RightGripperState_subscriber.Read() - self.gripper_sub_ready = True - if left_gripper_msg is not None and right_gripper_msg is not None: - self.left_gripper_state_value.value = left_gripper_msg.states[0].q - self.right_gripper_state_value.value = right_gripper_msg.states[0].q - time.sleep(0.002) - - def ctrl_dual_gripper(self, dual_gripper_action): - """set current left, right gripper motor cmd target q""" - self.left_gripper_msg.cmds[0].q = dual_gripper_action[0] - self.right_gripper_msg.cmds[0].q = dual_gripper_action[1] - - self.LeftGripperCmb_publisher.Write(self.left_gripper_msg) - self.RightGripperCmb_publisher.Write(self.right_gripper_msg) - # logger_mp.debug("gripper ctrl publish ok.") - - def control_thread( - self, - left_gripper_value_in, - right_gripper_value_in, - left_gripper_state_value, - right_gripper_state_value, - dual_hand_data_lock=None, - dual_gripper_state_out=None, - dual_gripper_action_out=None, - ): - self.running = True - LEFT_MAPPED_MIN = 0.0 # The minimum initial motor position when the gripper closes at startup. - RIGHT_MAPPED_MIN = 0.0 # The minimum initial motor position when the gripper closes at startup. - # The maximum initial motor position when the gripper closes before calibration (with the rail stroke calculated as 0.6 cm/rad * 9 rad = 5.4 cm). - - dq = 0.0 - tau = 0.0 - kp = 5.00 - kd = 0.05 - # initialize gripper cmd msg - self.left_gripper_msg = MotorCmds_() - self.left_gripper_msg.cmds = [unitree_go_msg_dds__MotorCmd_()] - self.right_gripper_msg = MotorCmds_() - self.right_gripper_msg.cmds = [unitree_go_msg_dds__MotorCmd_()] - - self.left_gripper_msg.cmds[0].dq = dq - self.left_gripper_msg.cmds[0].tau = tau - self.left_gripper_msg.cmds[0].kp = kp - self.left_gripper_msg.cmds[0].kd = kd - - self.right_gripper_msg.cmds[0].dq = dq - self.right_gripper_msg.cmds[0].tau = tau - self.right_gripper_msg.cmds[0].kp = kp - self.right_gripper_msg.cmds[0].kd = kd - try: - while self.running: - start_time = time.time() - # get dual hand skeletal point state from XR device - with left_gripper_value_in.get_lock(): - left_gripper_value = left_gripper_value_in.value - with right_gripper_value_in.get_lock(): - right_gripper_value = right_gripper_value_in.value - # get current dual gripper motor state - dual_gripper_state = np.array([left_gripper_state_value.value, right_gripper_state_value.value]) - dual_gripper_action = np.array([left_gripper_value, right_gripper_value]) - - if dual_gripper_state_out and dual_gripper_action_out: - with dual_hand_data_lock: - dual_gripper_state_out[:] = dual_gripper_state - np.array([LEFT_MAPPED_MIN, RIGHT_MAPPED_MIN]) - dual_gripper_action_out[:] = dual_gripper_action - np.array([LEFT_MAPPED_MIN, RIGHT_MAPPED_MIN]) - self.ctrl_dual_gripper(dual_gripper_action) - current_time = time.time() - time_elapsed = current_time - start_time - sleep_time = max(0, (1 / self.fps) - time_elapsed) - time.sleep(sleep_time) - finally: - logger_mp.info("Dex1_1_Gripper_Controller has been closed.") - - -class Gripper_JointIndex(IntEnum): - kGripper = 0 diff --git a/src/lerobot/robots/unitree_g1/eval_robot/teleop_sim.py b/src/lerobot/robots/unitree_g1/eval_robot/teleop_sim.py deleted file mode 100644 index cfd8fd772..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/teleop_sim.py +++ /dev/null @@ -1,98 +0,0 @@ -# unitree_lerobot/eval_robot/teleop_sim_min.py -import time -import traceback -import numpy as np - -from unitree_lerobot.lerobot.src.lerobot.teleoperators.homunculus import ( - HomunculusArm, HomunculusArmConfig -) -from unitree_lerobot.eval_robot.robot_control.robot_arm_test import G1_29_ArmController -from unitree_lerobot.eval_robot.robot_control.robot_arm_ik import G1_29_ArmIK - -# ---- map teleop (5 chans in [-1,1]) -> joint targets (rad) -# Order: S_pitch, S_yaw, S_roll, Elbow_flex, Wrist_roll -def scale_to_joint_limits(u5: np.ndarray) -> np.ndarray: - # Tweak these if your sim build expects the alternate set - mins = np.array([-3.05, 0.00, -2.30, -1.00, 1.95], dtype=np.float32) - maxs = np.array([ 2.65, 2.20, 2.30, 2.00, -1.00], dtype=np.float32) - u = np.clip(u5.astype(np.float32), -1.0, 1.0) - return mins + (u + 1.0) * 0.5 * (maxs - mins) - -def main(): - # --- Teleop (Homunculus) --- - EXO_PORT = "/dev/ttyACM0" # change if needed - EXO_ID = "unitree_left" - - exo_cfg = HomunculusArmConfig(EXO_PORT, id=EXO_ID) - exo = HomunculusArm(exo_cfg) - exo.connect(calibrate=True) - - # --- Robot (Simulation) --- - # No EvalRealConfig, no setup_robot_interface — direct construction - arm_ik = G1_29_ArmIK() - arm_ctrl = G1_29_ArmController(motion_mode=False, simulation_mode=True) - - # Determine DoF safely (fallback to 14 if not exposed) - arm_dof = getattr(arm_ctrl, "arm_dof", 14) - - # Optional: neutral pose - neutral = np.zeros(arm_dof, dtype=np.float32) - try: - tau0 = arm_ik.solve_tau(neutral) - arm_ctrl.ctrl_dual_arm(neutral, tau0) - except Exception: - pass - - # Control loop - HZ = 100.0 - DT = 1.0 / HZ - - # Simple smoothing to avoid jitter - q_prev = neutral.copy() - alpha = 0.35 # 0..1, higher = snappier - - try: - while True: - t0 = time.perf_counter() - - # 1) Read teleop dict (values ~ [-100, 100]); take first 5 channels - teleop = exo.get_action() # Ordered dict - vals = list(teleop.values())[:5] - - # Keep first, negate others (matches your earlier convention) - vals = [vals[0]] + [-v for v in vals[1:]] - - # Normalize to [-1, 1] - norm = np.asarray(vals, dtype=np.float32) / 100.0 - - # 2) Map to joint targets and assemble full arm command - q5 = scale_to_joint_limits(norm) - arm_cmd = np.zeros(arm_dof, dtype=np.float32) - arm_cmd[:5] = q5 - - # 3) Low-pass filter for smoothness - q_smooth = alpha * arm_cmd + (1.0 - alpha) * q_prev - q_prev = q_smooth - - # 4) Compute torques and send - tau = arm_ik.solve_tau(q_smooth) - arm_ctrl.ctrl_dual_arm(q_smooth, tau) - - # 5) rate control - elapsed = time.perf_counter() - t0 - if elapsed < DT: - time.sleep(DT - elapsed) - - except KeyboardInterrupt: - pass - except Exception: - traceback.print_exc() - finally: - try: - exo.disconnect() - except Exception: - pass - # Optionally: arm_ctrl.ctrl_dual_arm_go_home() - -if __name__ == "__main__": - main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/test.py b/src/lerobot/robots/unitree_g1/eval_robot/test.py deleted file mode 100644 index b06f1f17a..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/test.py +++ /dev/null @@ -1,40 +0,0 @@ -"""' -Refer to: lerobot/lerobot/scripts/eval.py - lerobot/lerobot/scripts/econtrol_robot.py - lerobot/robot_devices/control_utils.py -""" - -import time -import numpy as np -import cv2 -from multiprocessing.sharedctypes import SynchronizedArray -from lerobot.configs import parser -from lerobot.datasets.lerobot_dataset import LeRobotDataset -from eval_robot.make_robot import ( - setup_image_client, - setup_robot_interface, - process_images_and_observations, -) -from eval_robot.utils.utils import cleanup_resources, EvalRealConfig - -from eval_robot.utils.rerun_visualizer import RerunLogger, visualization_data -from eval_robot.utils.utils import to_list, to_scalar -from eval_robot.robot_control.robot_arm_test import ( - G1_29_ArmController, G1_29_JointIndex -) -import logging_mp -from eval_robot.robot_control.robot_arm_ik import G1_29_ArmIK - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def replay_main(): - - #damp needs to be on? do i start the robot as well - - arm_ik = G1_29_ArmIK() - arm_ctrl = G1_29_ArmController(motion_mode=False, simulation_mode=False)#motors move here upon init. there's a bug where when closing python the motors die - -if __name__ == "__main__": - replay_main() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/utils/episode_writer.py b/src/lerobot/robots/unitree_g1/eval_robot/utils/episode_writer.py deleted file mode 100644 index d31d7d679..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/utils/episode_writer.py +++ /dev/null @@ -1,219 +0,0 @@ -import os -import cv2 -import json -import datetime -import numpy as np -import time - -from queue import Queue, Empty -from threading import Thread -import logging_mp - -logger_mp = logging_mp.get_logger(__name__) - - -class EpisodeWriter: - def __init__(self, task_dir, frequency=30, image_size=[640, 480]): - """ - image_size: [width, height] - """ - logger_mp.info("==> EpisodeWriter initializing...\n") - self.task_dir = task_dir - self.frequency = frequency - self.image_size = image_size - - self.data = {} - self.episode_data = [] - self.item_id = -1 - self.episode_id = -1 - if os.path.exists(self.task_dir): - episode_dirs = [episode_dir for episode_dir in os.listdir(self.task_dir) if "episode_" in episode_dir] - episode_last = sorted(episode_dirs)[-1] if len(episode_dirs) > 0 else None - self.episode_id = 0 if episode_last is None else int(episode_last.split("_")[-1]) - logger_mp.info(f"==> task_dir directory already exist, now self.episode_id is:{self.episode_id}\n") - else: - os.makedirs(self.task_dir) - logger_mp.info("==> episode directory does not exist, now create one.\n") - self.data_info() - self.text_desc() - self.result = None - self.is_available = True # Indicates whether the class is available for new operations - # Initialize the queue and worker thread - self.item_data_queue = Queue(-1) - self.stop_worker = False - self.need_save = False # Flag to indicate when save_episode is triggered - self.worker_thread = Thread(target=self.process_queue) - self.worker_thread.start() - - logger_mp.info("==> EpisodeWriter initialized successfully.\n") - - def data_info(self, version="1.0.0", date=None, author=None): - self.info = { - "version": "1.0.0" if version is None else version, - "date": datetime.date.today().strftime("%Y-%m-%d") if date is None else date, - "author": "unitree" if author is None else author, - "image": {"width": self.image_size[0], "height": self.image_size[1], "fps": self.frequency}, - "depth": {"width": self.image_size[0], "height": self.image_size[1], "fps": self.frequency}, - "audio": {"sample_rate": 16000, "channels": 1, "format": "PCM", "bits": 16}, # PCM_S16 - "joint_names": { - "left_arm": [ - "kLeftShoulderPitch", - "kLeftShoulderRoll", - "kLeftShoulderYaw", - "kLeftElbow", - "kLeftWristRoll", - "kLeftWristPitch", - "kLeftWristyaw", - ], - "left_ee": [], - "right_arm": [], - "right_ee": [], - "body": [], - }, - "tactile_names": { - "left_ee": [], - "right_ee": [], - }, - "sim_state": "", - } - - def text_desc(self): - self.text = { - "goal": "Place the wooden blocks into the yellow frame, stacking them from bottom to top in the order: red, yellow, green.", - "desc": "Using the gripper, first place the red wooden block into the yellow frame. Next, stack the yellow wooden block on top of the red one, and finally place the green wooden block on top of the yellow block.", - "steps": "", - } - - def create_episode(self): - """ - Create a new episode. - Returns: - bool: True if the episode is successfully created, False otherwise. - Note: - Once successfully created, this function will only be available again after save_episode complete its save task. - """ - if not self.is_available: - logger_mp.info( - "==> The class is currently unavailable for new operations. Please wait until ongoing tasks are completed." - ) - return False # Return False if the class is unavailable - - # Reset episode-related data and create necessary directories - self.item_id = -1 - self.episode_data = [] - self.episode_id = self.episode_id + 1 - - self.episode_dir = os.path.join(self.task_dir, f"episode_{str(self.episode_id).zfill(4)}") - self.color_dir = os.path.join(self.episode_dir, "colors") - self.depth_dir = os.path.join(self.episode_dir, "depths") - self.audio_dir = os.path.join(self.episode_dir, "audios") - self.json_path = os.path.join(self.episode_dir, "data.json") - os.makedirs(self.episode_dir, exist_ok=True) - os.makedirs(self.color_dir, exist_ok=True) - os.makedirs(self.depth_dir, exist_ok=True) - os.makedirs(self.audio_dir, exist_ok=True) - - self.is_available = False # After the episode is created, the class is marked as unavailable until the episode is successfully saved - logger_mp.info(f"==> New episode created: {self.episode_dir}") - return True # Return True if the episode is successfully created - - def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None, sim_state=None): - # Increment the item ID - self.item_id += 1 - # Create the item data dictionary - item_data = { - "idx": self.item_id, - "colors": colors, - "depths": depths, - "states": states, - "actions": actions, - "tactiles": tactiles, - "audios": audios, - "sim_state": sim_state, - } - # Enqueue the item data - self.item_data_queue.put(item_data) - - def process_queue(self): - while not self.stop_worker or not self.item_data_queue.empty(): - # Process items in the queue - try: - item_data = self.item_data_queue.get(timeout=1) - try: - self._process_item_data(item_data) - except Exception as e: - logger_mp.info(f"Error processing item_data (idx={item_data['idx']}): {e}") - self.item_data_queue.task_done() - except Empty: - pass - - # Check if save_episode was triggered - if self.need_save and self.item_data_queue.empty(): - self._save_episode() - - def _process_item_data(self, item_data): - idx = item_data["idx"] - colors = item_data.get("colors", {}) - depths = item_data.get("depths", {}) - audios = item_data.get("audios", {}) - - # Save images - if colors: - for idx_color, (color_key, color) in enumerate(colors.items()): - color_name = f"{str(idx).zfill(6)}_{color_key}.jpg" - if not cv2.imwrite(os.path.join(self.color_dir, color_name), color): - logger_mp.info("Failed to save color image.") - item_data["colors"][color_key] = os.path.join("colors", color_name) - - # Save depths - if depths: - for idx_depth, (depth_key, depth) in enumerate(depths.items()): - depth_name = f"{str(idx).zfill(6)}_{depth_key}.jpg" - if not cv2.imwrite(os.path.join(self.depth_dir, depth_name), depth): - logger_mp.info("Failed to save depth image.") - item_data["depths"][depth_key] = os.path.join("depths", depth_name) - - # Save audios - if audios: - for mic, audio in audios.items(): - audio_name = f"audio_{str(idx).zfill(6)}_{mic}.npy" - np.save(os.path.join(self.audio_dir, audio_name), audio.astype(np.int16)) - item_data["audios"][mic] = os.path.join("audios", audio_name) - - # Update episode data - self.episode_data.append(item_data) - - def save_episode(self, result): - """ - Trigger the save operation. This sets the save flag, and the process_queue thread will handle it. - """ - self.need_save = True # Set the save flag - self.result = result - logger_mp.info("==> Episode saved start...") - - def _save_episode(self): - """ - Save the episode data to a JSON file. - """ - self.data["info"] = self.info - self.data["text"] = self.text - self.data["data"] = self.episode_data - self.data["result"] = self.result - - with open(self.json_path, "w", encoding="utf-8") as jsonf: - jsonf.write(json.dumps(self.data, indent=4, ensure_ascii=False)) - self.need_save = False # Reset the save flag - self.is_available = True # Mark the class as available after saving - logger_mp.info(f"==> Episode saved successfully to {self.json_path} with result: {self.result}") - - def close(self): - """ - Stop the worker thread and ensure all tasks are completed. - """ - self.item_data_queue.join() - if not self.is_available: # If self.is_available is False, it means there is still data not saved. - self.save_episode(self.result) - while not self.is_available: - time.sleep(0.01) - self.stop_worker = True - self.worker_thread.join() diff --git a/src/lerobot/robots/unitree_g1/eval_robot/utils/rerun_visualizer.py b/src/lerobot/robots/unitree_g1/eval_robot/utils/rerun_visualizer.py deleted file mode 100644 index 0e2dc1277..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/utils/rerun_visualizer.py +++ /dev/null @@ -1,166 +0,0 @@ -import torch -from datetime import datetime -from typing import Any - -import rerun as rr -import rerun.blueprint as rrb - - -class RerunLogger: - """ - A fully automatic Rerun logger designed to parse and visualize step - dictionaries directly from a LeRobotDataset. - """ - - def __init__( - self, - prefix: str = "", - memory_limit: str = "200MB", - idxrangeboundary: int | None = 300, - ): - """Initializes the Rerun logger.""" - # Use a descriptive name for the Rerun recording - rr.init(f"Dataset_Log_{datetime.now().strftime('%Y%m%d_%H%M%S')}") - rr.spawn(memory_limit=memory_limit) - - self.prefix = prefix - self.blueprint_sent = False - self.idxrangeboundary = idxrangeboundary - - # --- Internal cache for discovered keys --- - self._image_keys: tuple[str, ...] = () - self._state_key: str = "" - self._action_key: str = "" - self._index_key: str = "index" - self._task_key: str = "task" - self._episode_index_key: str = "episode_index" - - self.current_episode = -1 - - def _initialize_from_data(self, step_data: dict[str, Any]): - """Inspects the first data dictionary to discover components and set up the blueprint.""" - print("RerunLogger: First data packet received. Auto-configuring...") - - image_keys = [] - for key, value in step_data.items(): - if key.startswith("observation.images.") and isinstance(value, torch.Tensor) and value.ndim > 2: - image_keys.append(key) - elif key == "observation.state": - self._state_key = key - elif key == "action": - self._action_key = key - - self._image_keys = tuple(sorted(image_keys)) - - if "index" in step_data: - self._index_key = "index" - elif "frame_index" in step_data: - self._index_key = "frame_index" - - print(f" - Using '{self._index_key}' for time sequence.") - print(f" - Detected State Key: '{self._state_key}'") - print(f" - Detected Action Key: '{self._action_key}'") - print(f" - Detected Image Keys: {self._image_keys}") - if self.idxrangeboundary: - self.setup_blueprint() - - def setup_blueprint(self): - """Sets up and sends the Rerun blueprint based on detected components.""" - views = [] - - for key in self._image_keys: - clean_name = key.replace("observation.images.", "") - entity_path = f"{self.prefix}images/{clean_name}" - views.append(rrb.Spatial2DView(origin=entity_path, name=clean_name)) - - if self._state_key: - entity_path = f"{self.prefix}state" - views.append( - rrb.TimeSeriesView( - origin=entity_path, - name="Observation State", - time_ranges=[ - rrb.VisibleTimeRange( - "frame", - start=rrb.TimeRangeBoundary.cursor_relative(seq=-self.idxrangeboundary), - end=rrb.TimeRangeBoundary.cursor_relative(), - ) - ], - plot_legend=rrb.PlotLegend(visible=True), - ) - ) - - if self._action_key: - entity_path = f"{self.prefix}action" - views.append( - rrb.TimeSeriesView( - origin=entity_path, - name="Action", - time_ranges=[ - rrb.VisibleTimeRange( - "frame", - start=rrb.TimeRangeBoundary.cursor_relative(seq=-self.idxrangeboundary), - end=rrb.TimeRangeBoundary.cursor_relative(), - ) - ], - plot_legend=rrb.PlotLegend(visible=True), - ) - ) - - if not views: - print("Warning: No visualizable components detected in the data.") - return - - grid = rrb.Grid(contents=views) - rr.send_blueprint(grid) - self.blueprint_sent = True - - def log_step(self, step_data: dict[str, Any]): - """Logs a single step dictionary from your dataset.""" - if not self.blueprint_sent: - self._initialize_from_data(step_data) - - if self._index_key in step_data: - current_index = step_data[self._index_key].item() - rr.set_time_sequence("frame", current_index) - - episode_idx = step_data.get(self._episode_index_key, torch.tensor(-1)).item() - if episode_idx != self.current_episode: - self.current_episode = episode_idx - task_name = step_data.get(self._task_key, "Unknown Task") - log_text = f"Starting Episode {self.current_episode}: {task_name}" - rr.log(f"{self.prefix}info/task", rr.TextLog(log_text, level=rr.TextLogLevel.INFO)) - - for key in self._image_keys: - if key in step_data: - image_tensor = step_data[key] - if image_tensor.ndim > 2: - clean_name = key.replace("observation.images.", "") - entity_path = f"{self.prefix}images/{clean_name}" - if image_tensor.shape[0] in [1, 3, 4]: - image_tensor = image_tensor.permute(1, 2, 0) - rr.log(entity_path, rr.Image(image_tensor)) - - if self._state_key in step_data: - state_tensor = step_data[self._state_key] - entity_path = f"{self.prefix}state" - for i, val in enumerate(state_tensor): - rr.log(f"{entity_path}/joint_{i}", rr.Scalar(val.item())) - - if self._action_key in step_data: - action_tensor = step_data[self._action_key] - entity_path = f"{self.prefix}action" - for i, val in enumerate(action_tensor): - rr.log(f"{entity_path}/joint_{i}", rr.Scalar(val.item())) - - -def visualization_data(idx, observation, state, action, online_logger): - item_data: dict[str, Any] = { - "index": torch.tensor(idx), - "observation.state": state, - "action": action, - } - for k, v in observation.items(): - if k not in ("index", "observation.state", "action"): - item_data[k] = v - online_logger.log_step(item_data) diff --git a/src/lerobot/robots/unitree_g1/eval_robot/utils/sim_savedata_utils.py b/src/lerobot/robots/unitree_g1/eval_robot/utils/sim_savedata_utils.py deleted file mode 100644 index 97f82adde..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/utils/sim_savedata_utils.py +++ /dev/null @@ -1,209 +0,0 @@ -# for simulation -import torch -import numpy as np -import logging_mp -from unitree_lerobot.eval_robot.utils.utils import ( - reset_policy, -) -from unitree_lerobot.eval_robot.make_robot import ( - publish_reset_category, -) -from dataclasses import dataclass -from lerobot.configs import parser -from lerobot.configs.policies import PreTrainedConfig -import time - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def process_data_add(episode_writer, observation_image, current_arm_q, ee_state, action, arm_dof, ee_dof): - if episode_writer is None: - return - if ( - observation_image is not None - and current_arm_q is not None - and ee_state is not None - and action is not None - and arm_dof is not None - and ee_dof is not None - ): - # Convert tensors to numpy arrays for JSON serialization - if torch.is_tensor(current_arm_q): - current_arm_q = current_arm_q.detach().cpu().numpy() - if torch.is_tensor(ee_state): - ee_state = ee_state.detach().cpu().numpy() - if torch.is_tensor(action): - action = action.detach().cpu().numpy() - colors = {} - i = 0 - for key, value in observation_image.items(): - if "images" in key: - if value is not None: - # Convert PyTorch tensor to numpy array for OpenCV compatibility - if torch.is_tensor(value): - # Convert tensor to numpy array and ensure correct format for OpenCV - img_array = value.detach().cpu().numpy() - # If the image is in CHW format (channels first), convert to HWC format (channels last) - if img_array.ndim == 3 and img_array.shape[0] in [1, 3, 4]: - img_array = np.transpose(img_array, (1, 2, 0)) - # Ensure the array is in uint8 format for OpenCV - if img_array.dtype != np.uint8: - if img_array.max() <= 1.0: # Normalized values [0, 1] - img_array = (img_array * 255).astype(np.uint8) - else: # Values already in [0, 255] range - img_array = img_array.astype(np.uint8) - # Keep original RGB format - no color channel conversion needed - colors[f"color_{i}"] = img_array - else: - colors[f"color_{i}"] = value - i += 1 - states = { - "left_arm": { - "qpos": current_arm_q[: arm_dof // 2].tolist(), # numpy.array -> list - "qvel": [], - "torque": [], - }, - "right_arm": { - "qpos": current_arm_q[arm_dof // 2 :].tolist(), - "qvel": [], - "torque": [], - }, - "left_ee": { - "qpos": ee_state[:ee_dof].tolist(), - "qvel": [], - "torque": [], - }, - "right_ee": { - "qpos": ee_state[ee_dof:].tolist(), - "qvel": [], - "torque": [], - }, - "body": { - "qpos": [], - }, - } - actions = { - "left_arm": { - "qpos": action[: arm_dof // 2].tolist(), - "qvel": [], - "torque": [], - }, - "right_arm": { - "qpos": action[arm_dof // 2 :].tolist(), - "qvel": [], - "torque": [], - }, - "left_ee": { - "qpos": action[arm_dof : arm_dof + ee_dof].tolist(), - "qvel": [], - "torque": [], - }, - "right_ee": { - "qpos": action[arm_dof + ee_dof : arm_dof + 2 * ee_dof].tolist(), - "qvel": [], - "torque": [], - }, - "body": { - "qpos": [], - }, - } - episode_writer.add_item(colors, states=states, actions=actions) - - -def process_data_save(episode_writer, result): - """Processes data and saves it.""" - if episode_writer is None: - return - episode_writer.save_episode(result) - - -def is_success( - sim_reward_subscriber, - episode_writer, - reset_pose_publisher, - policy, - cfg, - reward_stats, - init_arm_pose, - robot_interface, -): - # logger_mp.info(f"arm_action {arm_action}, tau {tau}") - if sim_reward_subscriber: - data = sim_reward_subscriber.read_data() - if data is not None: - if int(data["rewards"][0]) == 1: - reward_stats["reward_sum"] += 1 - sim_reward_subscriber.reset_data() - # success - if reward_stats["reward_sum"] >= 25: - process_data_save(episode_writer, "success") - logger_mp.info( - f"Episode {reward_stats['episode_num']} finished with reward {reward_stats['reward_sum']},save data..." - ) - reward_stats["episode_num"] = -1 - reward_stats["reward_sum"] = 0 - time.sleep(1) - publish_reset_category(1, reset_pose_publisher) - time.sleep(1) - reset_policy(policy) - sim_reward_subscriber.reset_data() - # fail - elif reward_stats["episode_num"] > cfg.max_episodes: - process_data_save(episode_writer, "fail") - logger_mp.info(f"Episode {reward_stats['episode_num']} finished with reward {reward_stats['reward_sum']}") - reward_stats["episode_num"] = -1 - reward_stats["reward_sum"] = 0 - reset_policy(policy) - sim_reward_subscriber.reset_data() - logger_mp.info("Initializing robot to starting pose...") - tau = robot_interface["arm_ik"].solve_tau(init_arm_pose) - robot_interface["arm_ctrl"].ctrl_dual_arm(init_arm_pose, tau) - time.sleep(1) - publish_reset_category(1, reset_pose_publisher) - time.sleep(1) - reset_policy(policy) - sim_reward_subscriber.reset_data() - time.sleep(1) - - -@dataclass -class EvalRealConfig: - repo_id: str - policy: PreTrainedConfig | None = None - - root: str = "" - episodes: int = 0 - frequency: float = 30.0 - - # Basic control parameters - arm: str = "G1_29" # G1_29, G1_23 - ee: str = "dex3" # dex3, dex1, inspire1, brainco - - # Mode flags - motion: bool = False - headless: bool = False - sim: bool = True - visualization: bool = False - send_real_robot: bool = False - use_dataset: bool = False - save_data: bool = False - task_dir: str = "./data" - max_episodes: int = 1200 - - def __post_init__(self): - # HACK: We parse again the cli args here to get the pretrained path if there was one. - policy_path = parser.get_path_arg("policy") - if policy_path: - cli_overrides = parser.get_cli_overrides("policy") - self.policy = PreTrainedConfig.from_pretrained(policy_path, cli_overrides=cli_overrides) - self.policy.pretrained_path = policy_path - else: - logger_mp.warning( - "No pretrained path was provided, evaluated policy will be built from scratch (random weights)." - ) - - @classmethod - def __get_path_fields__(cls) -> list[str]: - """This enables the parser to load config from the policy using `--policy.path=local/dir`""" - return ["policy"] diff --git a/src/lerobot/robots/unitree_g1/eval_robot/utils/sim_state_topic.py b/src/lerobot/robots/unitree_g1/eval_robot/utils/sim_state_topic.py deleted file mode 100644 index 110280baa..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/utils/sim_state_topic.py +++ /dev/null @@ -1,402 +0,0 @@ -# Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. -# License: Apache License, Version 2.0 -""" -Simple sim state subscriber class -Subscribe to rt/sim_state_cmd topic and write to shared memory -""" - -import threading -import time -import json -from multiprocessing import shared_memory -from typing import Any -from unitree_sdk2py.core.channel import ChannelSubscriber -from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ - -import logging_mp - -logger_mp = logging_mp.get_logger(__name__) - - -class SharedMemoryManager: - """Shared memory manager""" - - def __init__(self, name: str | None = None, size: int = 512): - """Initialize shared memory manager - - Args: - name: shared memory name, if None, create new one - size: shared memory size (bytes) - """ - self.size = size - self.lock = threading.RLock() # reentrant lock - - if name: - try: - self.shm = shared_memory.SharedMemory(name=name) - self.shm_name = name - self.created = False - except FileNotFoundError: - self.shm = shared_memory.SharedMemory(create=True, size=size) - self.shm_name = self.shm.name - self.created = True - else: - self.shm = shared_memory.SharedMemory(create=True, size=size) - self.shm_name = self.shm.name - self.created = True - - def write_data(self, data: dict[str, Any]) -> bool: - """Write data to shared memory - - Args: - data: data to write - - Returns: - bool: write success or not - """ - try: - with self.lock: - json_str = json.dumps(data) - json_bytes = json_str.encode("utf-8") - - if len(json_bytes) > self.size - 8: # reserve 8 bytes for length and timestamp - logger_mp.warning(f"Data too large for shared memory ({len(json_bytes)} > {self.size - 8})") - return False - - # write timestamp (4 bytes) and data length (4 bytes) - timestamp = int(time.time()) & 0xFFFFFFFF # 32-bit timestamp, use bitmask to ensure in range - self.shm.buf[0:4] = timestamp.to_bytes(4, "little") - self.shm.buf[4:8] = len(json_bytes).to_bytes(4, "little") - - # write data - self.shm.buf[8 : 8 + len(json_bytes)] = json_bytes - return True - - except Exception as e: - logger_mp.error(f"Error writing to shared memory: {e}") - return False - - def read_data(self) -> dict[str, Any] | None: - """Read data from shared memory - - Returns: - Dict[str, Any]: read data dictionary, return None if failed - """ - try: - with self.lock: - # read timestamp and data length - timestamp = int.from_bytes(self.shm.buf[0:4], "little") - data_len = int.from_bytes(self.shm.buf[4:8], "little") - - if data_len == 0: - return None - - # read data - json_bytes = bytes(self.shm.buf[8 : 8 + data_len]) - data = json.loads(json_bytes.decode("utf-8")) - data["_timestamp"] = timestamp # add timestamp information - return data - - except Exception as e: - logger_mp.error(f"Error reading from shared memory: {e}") - return None - - def reset_data(self): - """Reset data""" - if self.shm: - self.shm.buf[0:8] = b"\x00" * 8 - else: - logger_mp.error("[SharedMemoryManager] Shared memory is not initialized") - - def get_name(self) -> str: - """Get shared memory name""" - return self.shm_name - - def cleanup(self): - """Clean up shared memory""" - if hasattr(self, "shm") and self.shm: - self.shm.close() - if self.created: - self.shm.unlink() - - def __del__(self): - """Destructor""" - self.cleanup() - - -class SimStateSubscriber: - """Simple sim state subscriber class""" - - def __init__(self, shm_name: str = "sim_state_cmd_data", shm_size: int = 4096): - """Initialize the subscriber - - Args: - shm_name: shared memory name - shm_size: shared memory size - """ - self.shm_name = shm_name - self.shm_size = shm_size - self.running = False - self.subscriber = None - self.subscribe_thread = None - self.shared_memory = None - - # initialize shared memory - self._setup_shared_memory() - - logger_mp.debug(f"[SimStateSubscriber] Initialized with shared memory: {shm_name}") - - def _setup_shared_memory(self): - """Setup shared memory""" - try: - self.shared_memory = SharedMemoryManager(self.shm_name, self.shm_size) - logger_mp.debug("[SimStateSubscriber] Shared memory setup successfully") - except Exception as e: - logger_mp.error(f"[SimStateSubscriber] Failed to setup shared memory: {e}") - - def start_subscribe(self): - """Start subscribing""" - if self.running: - logger_mp.warning("[SimStateSubscriber] Already running") - return - - try: - self.subscriber = ChannelSubscriber("rt/sim_state", String_) - self.subscriber.Init() - self.running = True - - self.subscribe_thread = threading.Thread(target=self._subscribe_sim_state, daemon=True) - self.subscribe_thread.start() - - logger_mp.info("[SimStateSubscriber] Started subscribing to rt/sim_state") - - except Exception as e: - logger_mp.error(f"[SimStateSubscriber] Failed to start subscribing: {e}") - self.running = False - - def _subscribe_sim_state(self): - """Subscribe loop thread""" - logger_mp.debug("[SimStateSubscriber] Subscribe thread started") - - while self.running: - try: - if self.subscriber: - msg = self.subscriber.Read() - if msg: - data = json.loads(msg.data) - else: - logger_mp.warning("[SimStateSubscriber] Received None message") - if self.shared_memory and data: - self.shared_memory.write_data(data) - else: - logger_mp.error("[SimStateSubscriber] Subscriber is not initialized") - time.sleep(0.002) - except Exception as e: - logger_mp.error(f"[SimStateSubscriber] Error in subscribe loop: {e}") - time.sleep(0.01) - - def stop_subscribe(self): - """Stop subscribing""" - if not self.running: - logger_mp.warning("[SimStateSubscriber] Already stopped or not running") - return - - self.running = False - # wait for thread to finish - if self.subscribe_thread: - self.subscribe_thread.join(timeout=1.0) - - if self.shared_memory: - self.shared_memory.cleanup() - logger_mp.info("[SimStateSubscriber] Subscriber stopped") - - def read_data(self) -> dict[str, Any] | None: - """Read data from shared memory - - Returns: - Dict: received data, None if no data or error - """ - try: - if self.shared_memory: - return self.shared_memory.read_data() - return None - except Exception as e: - logger_mp.error(f"[SimStateSubscriber] Error reading data: {e}") - return None - - def is_running(self) -> bool: - """Check if subscriber is running""" - return self.running - - def __del__(self): - """Destructor""" - self.stop_subscribe() - - -def start_sim_state_subscribe(shm_name: str = "sim_state_cmd_data", shm_size: int = 4096) -> SimStateSubscriber: - """Start sim state subscribing - - Args: - shm_name: shared memory name - shm_size: shared memory size - - Returns: - SimStateSubscriber: started subscriber instance - """ - subscriber = SimStateSubscriber(shm_name, shm_size) - subscriber.start_subscribe() - return subscriber - - -# ============================== sim reward topic ============================== -class SimRewardSubscriber: - """Simple sim state subscriber class""" - - def __init__(self, shm_name: str = "sim_reward_cmd_data", shm_size: int = 256): - """Initialize the subscriber - - Args: - shm_name: shared memory name - shm_size: shared memory size - """ - self.shm_name = shm_name - self.shm_size = shm_size - self.running = False - self.subscriber = None - self.subscribe_thread = None - self.shared_memory = None - - # initialize shared memory - self._setup_shared_memory() - - logger_mp.debug(f"[SimRewardSubscriber] Initialized with shared memory: {shm_name}") - - def _setup_shared_memory(self): - """Setup shared memory""" - try: - self.shared_memory = SharedMemoryManager(self.shm_name, self.shm_size) - logger_mp.debug("[SimRewardSubscriber] Shared memory setup successfully") - except Exception as e: - logger_mp.error(f"[SimRewardSubscriber] Failed to setup shared memory: {e}") - - def start_subscribe(self): - """Start subscribing""" - if self.running: - logger_mp.warning("[SimRewardSubscriber] Already running") - return - - try: - self.subscriber = ChannelSubscriber("rt/rewards_state", String_) - self.subscriber.Init() - self.running = True - - self.subscribe_thread = threading.Thread(target=self._subscribe_sim_reward, daemon=True) - self.subscribe_thread.start() - - logger_mp.info("[SimRewardSubscriber] Started subscribing to rt/sim_reward") - - except Exception as e: - logger_mp.error(f"[SimRewardSubscriber] Failed to start subscribing: {e}") - self.running = False - - def _subscribe_sim_reward(self): - """Subscribe loop thread""" - logger_mp.debug("[SimRewardSubscriber] Subscribe thread started") - - while self.running: - try: - if self.subscriber: - msg = self.subscriber.Read() - if msg: - data = json.loads(msg.data) - else: - logger_mp.warning("[SimRewardSubscriber] Received None message") - if self.shared_memory and data: - self.shared_memory.write_data(data) - else: - logger_mp.error("[SimRewardSubscriber] Subscriber is not initialized") - time.sleep(0.01) - except Exception as e: - logger_mp.error(f"[SimRewardSubscriber] Error in subscribe loop: {e}") - time.sleep(0.02) - - def stop_subscribe(self): - """Stop subscribing""" - if not self.running: - logger_mp.warning("[SimRewardSubscriber] Already stopped or not running") - return - - self.running = False - # wait for thread to finish - if self.subscribe_thread: - self.subscribe_thread.join(timeout=1.0) - - if self.shared_memory: - self.shared_memory.cleanup() - logger_mp.info("[SimRewardSubscriber] Subscriber stopped") - - def read_data(self) -> dict[str, Any] | None: - """Read data from shared memory - - Returns: - Dict: received data, None if no data or error - """ - try: - if self.shared_memory: - return self.shared_memory.read_data() - return None - except Exception as e: - logger_mp.error(f"[SimRewardSubscriber] Error reading data: {e}") - return None - - def reset_data(self): - """Reset data""" - if self.shared_memory: - data = {"rewards": [0.0], "timestamp": 1758009108.266387} - self.shared_memory.write_data(data) - - def is_running(self) -> bool: - """Check if subscriber is running""" - return self.running - - def __del__(self): - """Destructor""" - self.stop_subscribe() - - -# ============================== sim reward topic ============================== -def start_sim_reward_subscribe(shm_name: str = "sim_reward_cmd_data", shm_size: int = 256) -> SimRewardSubscriber: - """Start sim reward subscribing - - Args: - shm_name: shared memory name - shm_size: shared memory size - - Returns: - SimRewardSubscriber: started subscriber instance - """ - subscriber = SimRewardSubscriber(shm_name, shm_size) - subscriber.start_subscribe() - return subscriber - - -# if __name__ == "__main__": -# # example usage -# logger_mp.info("Starting sim state subscriber...") -# ChannelFactoryInitialize(0) -# # create and start subscriber -# subscriber = start_sim_state_subscribe() - -# try: -# # keep running and check for data -# while True: -# data = subscriber.read_data() -# if data: -# logger_mp.info(f"Read data: {data}") -# time.sleep(1) - -# except KeyboardInterrupt: -# logger_mp.warning("\nInterrupted by user") -# finally: -# subscriber.stop_subscribe() -# logger_mp.info("Subscriber stopped") diff --git a/src/lerobot/robots/unitree_g1/eval_robot/utils/utils.py b/src/lerobot/robots/unitree_g1/eval_robot/utils/utils.py deleted file mode 100644 index 36d2c44d3..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/utils/utils.py +++ /dev/null @@ -1,142 +0,0 @@ -import numpy as np -import torch -from typing import Any -from contextlib import nullcontext -from copy import copy -import logging -from dataclasses import dataclass -from lerobot.configs import parser -from lerobot.configs.policies import PreTrainedConfig -from lerobot.policies.pretrained import PreTrainedPolicy - - -import logging_mp - -logging_mp.basic_config(level=logging_mp.INFO) -logger_mp = logging_mp.get_logger(__name__) - - -def extract_observation(step: dict): - observation = {} - - for key, value in step.items(): - if key.startswith("observation.images."): - if isinstance(value, np.ndarray) and value.ndim == 3 and value.shape[-1] in [1, 3]: - value = np.transpose(value, (2, 0, 1)) - observation[key] = value - - elif key == "observation.state": - observation[key] = value - - return observation - - -def predict_action( - observation: dict[str, np.ndarray], - policy: PreTrainedPolicy, - device: torch.device, - use_amp: bool, - task: str | None = None, - use_dataset: bool | None = False, -): - observation = copy(observation) - with ( - torch.inference_mode(), - torch.autocast(device_type=device.type) if device.type == "cuda" and use_amp else nullcontext(), - ): - # Convert to pytorch format: channel first and float32 in [0,1] with batch dimension - for name in observation: - if not use_dataset: - # Skip non-tensor observations (like task strings) - if not hasattr(observation[name], "unsqueeze"): - continue - if "images" in name: - observation[name] = observation[name].type(torch.float32) / 255 - observation[name] = observation[name].permute(2, 0, 1).contiguous() - - observation[name] = observation[name].unsqueeze(0).to(device) - - observation["task"] = [task if task else ""] - - # Compute the next action with the policy - # based on the current observation - action = policy.select_action(observation) - - # Remove batch dimension - action = action.squeeze(0) - - # Move to cpu, if not already the case - action = action.to("cpu") - - return action - - -def reset_policy(policy: PreTrainedPolicy): - policy.reset() - - -def cleanup_resources(image_info: dict[str, Any]): - """Safely close and unlink shared memory resources.""" - logger_mp.info("Cleaning up shared memory resources.") - for shm in image_info["shm_resources"]: - if shm: - shm.close() - shm.unlink() - - -def to_list(x): - if torch is not None and isinstance(x, torch.Tensor): - return x.detach().cpu().ravel().tolist() - if isinstance(x, np.ndarray): - return x.ravel().tolist() - if isinstance(x, (list, tuple)): - return list(x) - return [x] - - -def to_scalar(x): - if torch is not None and isinstance(x, torch.Tensor): - return float(x.detach().cpu().ravel()[0].item()) - if isinstance(x, np.ndarray): - return float(x.ravel()[0]) - if isinstance(x, (list, tuple)): - return float(x[0]) - return float(x) - - -@dataclass -class EvalRealConfig: - repo_id: str - policy: PreTrainedConfig | None = None - - root: str = "" - episodes: int = 0 - frequency: float = 30.0 - - # Basic control parameters - arm: str = "G1_29" # G1_29, G1_23 - ee: str = "dex3" # dex3, dex1, inspire1, brainco - - # Mode flags - motion: bool = False - headless: bool = False - visualization: bool = False - send_real_robot: bool = False - use_dataset: bool = False - - def __post_init__(self): - # HACK: We parse again the cli args here to get the pretrained path if there was one. - policy_path = parser.get_path_arg("policy") - if policy_path: - cli_overrides = parser.get_cli_overrides("policy") - self.policy = PreTrainedConfig.from_pretrained(policy_path, cli_overrides=cli_overrides) - self.policy.pretrained_path = policy_path - else: - logging.warning( - "No pretrained path was provided, evaluated policy will be built from scratch (random weights)." - ) - - @classmethod - def __get_path_fields__(cls) -> list[str]: - """This enables the parser to load config from the policy using `--policy.path=local/dir`""" - return ["policy"] diff --git a/src/lerobot/robots/unitree_g1/eval_robot/utils/weighted_moving_filter.py b/src/lerobot/robots/unitree_g1/eval_robot/utils/weighted_moving_filter.py deleted file mode 100644 index 20bf8806e..000000000 --- a/src/lerobot/robots/unitree_g1/eval_robot/utils/weighted_moving_filter.py +++ /dev/null @@ -1,99 +0,0 @@ -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() - - -if __name__ == "__main__": - # windows_size and weights - filter_params = [ - (np.array([0.7, 0.2, 0.1])), - (np.array([0.5, 0.3, 0.2])), - (np.array([0.4, 0.3, 0.2, 0.1])), - ] - - visualize_filter_comparison(filter_params, steps=100) diff --git a/src/lerobot/robots/unitree_g1/hope_jr_arm copy.py b/src/lerobot/robots/unitree_g1/hope_jr_arm copy.py deleted file mode 100644 index 220a29f8c..000000000 --- a/src/lerobot/robots/unitree_g1/hope_jr_arm copy.py +++ /dev/null @@ -1,176 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2025 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import logging -import time -from functools import cached_property -from typing import Any - -from lerobot.cameras.utils import make_cameras_from_configs -from lerobot.motors import Motor, MotorNormMode -from lerobot.motors.calibration_gui import RangeFinderGUI -from lerobot.motors.feetech import ( - FeetechMotorsBus, -) -from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError - -from ..robot import Robot -from ..utils import ensure_safe_goal_position -from .config_hope_jr import HopeJrArmConfig - -logger = logging.getLogger(__name__) - - -class HopeJrArm(Robot): - config_class = HopeJrArmConfig - name = "hope_jr_arm" - - def __init__(self, config: HopeJrArmConfig): - super().__init__(config) - self.config = config - self.bus = FeetechMotorsBus( - port=self.config.port, - motors={ - "shoulder_pitch": Motor(1, "sm8512bl", MotorNormMode.RANGE_M100_100), - "shoulder_yaw": Motor(2, "sts3250", MotorNormMode.RANGE_M100_100), - "shoulder_roll": Motor(3, "sts3250", MotorNormMode.RANGE_M100_100), - "elbow_flex": Motor(4, "sts3250", MotorNormMode.RANGE_M100_100), - "wrist_roll": Motor(5, "sts3250", MotorNormMode.RANGE_M100_100), - "wrist_yaw": Motor(6, "sts3250", MotorNormMode.RANGE_M100_100), - "wrist_pitch": Motor(7, "sts3250", MotorNormMode.RANGE_M100_100), - }, - calibration=self.calibration, - ) - self.cameras = make_cameras_from_configs(config.cameras) - - # HACK - self.shoulder_pitch = "shoulder_pitch" - self.other_motors = [m for m in self.bus.motors if m != "shoulder_pitch"] - - @property - def _motors_ft(self) -> dict[str, type]: - return {f"{motor}.pos": float for motor in self.bus.motors} - - @property - def _cameras_ft(self) -> dict[str, tuple]: - return { - cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras - } - - @cached_property - def observation_features(self) -> dict[str, type | tuple]: - return {**self._motors_ft, **self._cameras_ft} - - @cached_property - def action_features(self) -> dict[str, type]: - return self._motors_ft - - @property - def is_connected(self) -> bool: - return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) - - def connect(self, calibrate: bool = True) -> None: - """ - We assume that at connection time, arm is in a rest position, - and torque can be safely disabled to run calibration. - """ - if self.is_connected: - raise DeviceAlreadyConnectedError(f"{self} already connected") - - self.bus.connect(handshake=False) - if not self.is_calibrated and calibrate: - self.calibrate() - - # Connect the cameras - for cam in self.cameras.values(): - cam.connect() - - self.configure() - logger.info(f"{self} connected.") - - @property - def is_calibrated(self) -> bool: - return self.bus.is_calibrated - - def calibrate(self) -> None: - groups = { - "all": list(self.bus.motors.keys()), - "shoulder": ["shoulder_pitch", "shoulder_yaw", "shoulder_roll"], - "elbow": ["elbow_flex"], - "wrist": ["wrist_roll", "wrist_yaw", "wrist_pitch"], - } - - self.calibration = RangeFinderGUI(self.bus, groups).run() - self._save_calibration() - print("Calibration saved to", self.calibration_fpath) - - def configure(self) -> None: - with self.bus.torque_disabled(): - self.bus.configure_motors(maximum_acceleration=30, acceleration=30) - - def setup_motors(self) -> None: - # TODO: add docstring - for motor in reversed(self.bus.motors): - input(f"Connect the controller board to the '{motor}' motor only and press enter.") - self.bus.setup_motor(motor) - print(f"'{motor}' motor id set to {self.bus.motors[motor].id}") - - def get_observation(self) -> dict[str, Any]: - if not self.is_connected: - raise DeviceNotConnectedError(f"{self} is not connected.") - - # Read arm position - start = time.perf_counter() - obs_dict = self.bus.sync_read("Present_Position", self.other_motors) - obs_dict[self.shoulder_pitch] = self.bus.read("Present_Position", self.shoulder_pitch) - obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()} - dt_ms = (time.perf_counter() - start) * 1e3 - logger.debug(f"{self} read state: {dt_ms:.1f}ms") - - # Capture images from cameras - for cam_key, cam in self.cameras.items(): - start = time.perf_counter() - obs_dict[cam_key] = cam.async_read() - dt_ms = (time.perf_counter() - start) * 1e3 - logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms") - - return obs_dict - - def send_action(self, action: dict[str, Any]) -> dict[str, Any]: - if not self.is_connected: - raise DeviceNotConnectedError(f"{self} is not connected.") - - goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} - - # Cap goal position when too far away from present position. - # /!\ Slower fps expected due to reading from the follower. - if self.config.max_relative_target is not None: - present_pos = self.bus.sync_read("Present_Position") - goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()} - goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) - - self.bus.sync_write("Goal_Position", goal_pos) - return {f"{motor}.pos": val for motor, val in goal_pos.items()} - - def disconnect(self): - if not self.is_connected: - raise DeviceNotConnectedError(f"{self} is not connected.") - - self.bus.disconnect(self.config.disable_torque_on_disconnect) - for cam in self.cameras.values(): - cam.disconnect() - - logger.info(f"{self} disconnected.") diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index ccc39cdd2..fd43a0c80 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -1,7 +1,9 @@ import logging import time +import struct from functools import cached_property from typing import Any +from pathlib import Path from lerobot.cameras.utils import make_cameras_from_configs from lerobot.motors import Motor, MotorNormMode @@ -24,6 +26,7 @@ from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, Cha from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as hg_LowCmd, LowState_ as hg_LowState # idl for g1, h1_2 from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ from unitree_sdk2py.utils.crc import CRC +from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as go_LowCmd, LowState_ as go_LowState # idl for h1 from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ @@ -49,11 +52,23 @@ class MotorState: def __init__(self): self.q = None self.dq = None + self.tau_est = None # Estimated torque + self.temperature = None # Motor temperature + + +class IMUState: + def __init__(self): + self.quaternion = None # [w, x, y, z] + self.gyroscope = None # [x, y, z] angular velocity (rad/s) + self.accelerometer = None # [x, y, z] linear acceleration (m/s²) + self.rpy = None # [roll, pitch, yaw] (rad) + self.temperature = None # IMU temperature class G1_29_LowState: def __init__(self): self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)] + self.imu_state = IMUState() class DataBuffer: def __init__(self): @@ -105,8 +120,11 @@ class UnitreeG1(Robot): self.freeze_body = config.freeze_body self.gravity_compensation = config.gravity_compensation + self.calibrated = False + self.calibrate() + self.arm_ik = G1_29_ArmIK() # initialize lowcmd publisher and lowstate subscriber @@ -134,6 +152,9 @@ class UnitreeG1(Robot): logger_mp.warning("[UnitreeG1] Waiting to subscribe dds...") logger_mp.info("[UnitreeG1] Subscribe dds ok.") + # initialize audio client for LED, TTS, and audio playback + + # initialize hg's lowcmd msg self.crc = CRC() self.msg = unitree_hg_msg_dds__LowCmd_() @@ -166,6 +187,13 @@ class UnitreeG1(Robot): self.msg.motor_cmd[id].q = self.all_motor_q[id] #print current motor q, kp, kd + + if config.audio_client: + self.audio_client = AudioClient() + self.audio_client.SetTimeout(10.0) + self.audio_client.Init() + logger_mp.info("[UnitreeG1] Audio client initialized!") + logger_mp.info("Lock OK!\n") #motors are not locked x # for i in range(10000): # print(self.get_current_motor_q()) @@ -180,14 +208,33 @@ class UnitreeG1(Robot): def _subscribe_motor_state(self): while True: + start_time = time.time() msg = self.lowstate_subscriber.Read() if msg is not None: lowstate = G1_29_LowState() + + # Capture motor states for id in range(G1_29_Num_Motors): lowstate.motor_state[id].q = msg.motor_state[id].q lowstate.motor_state[id].dq = msg.motor_state[id].dq + lowstate.motor_state[id].tau_est = msg.motor_state[id].tau_est + lowstate.motor_state[id].temperature = msg.motor_state[id].temperature + + # Capture IMU state + lowstate.imu_state.quaternion = list(msg.imu_state.quaternion) + lowstate.imu_state.gyroscope = list(msg.imu_state.gyroscope) + lowstate.imu_state.accelerometer = list(msg.imu_state.accelerometer) + lowstate.imu_state.rpy = list(msg.imu_state.rpy) + lowstate.imu_state.temperature = msg.imu_state.temperature + self.lowstate_buffer.SetData(lowstate) + current_time = time.time() + all_t_elapsed = current_time - start_time + sleep_time = max(0, (self.control_dt - all_t_elapsed))#maintina constant control dt + time.sleep(sleep_time) + + def clip_arm_q_target(self, target_q, velocity_limit): current_q = self.get_current_dual_arm_q() delta = target_q - current_q @@ -334,6 +381,198 @@ class UnitreeG1(Robot): cam.disconnect() logger_mp.info(f"{self} disconnected.") + def get_full_robot_state(self) -> dict[str, Any]: + """ + Get full robot state including IMU and extended motor data. + + Returns: + dict with keys: + - 'imu': dict containing IMU data (quaternion, gyroscope, accelerometer, rpy, temperature) + - 'motors': list of dicts, one per motor, containing q, dq, tau_est, temperature + """ + lowstate = self.lowstate_buffer.GetData() + if lowstate is None: + raise RuntimeError("No robot state available. Is the robot connected?") + + # Extract IMU data + imu_data = { + 'quaternion': lowstate.imu_state.quaternion, # [w, x, y, z] + 'gyroscope': lowstate.imu_state.gyroscope, # [x, y, z] rad/s + 'accelerometer': lowstate.imu_state.accelerometer, # [x, y, z] m/s² + 'rpy': lowstate.imu_state.rpy, # [roll, pitch, yaw] rad + 'temperature': lowstate.imu_state.temperature, # °C + } + + # Extract motor data + motors_data = [] + for i in range(G1_29_Num_Motors): + motor = lowstate.motor_state[i] + motors_data.append({ + 'id': i, + 'q': motor.q, # position (rad) + 'dq': motor.dq, # velocity (rad/s) + 'tau_est': motor.tau_est, # estimated torque (Nm) + 'temperature': motor.temperature[0] if isinstance(motor.temperature, (list, tuple)) else motor.temperature, # °C + }) + + return { + 'imu': imu_data, + 'motors': motors_data, + } + + def audio_control(self, command, volume: int = 80): + """ + Unified audio/LED control function for the G1 robot. + + Args: + command: Can be one of: + - str: Text to speak via TTS + - tuple[int, int, int]: RGB values (0-255) for LED control + - str (path): Path to WAV file to play + volume: Volume level 0-100 (default: 80) + + Examples: + robot.audio_control("Hello world") # TTS + robot.audio_control((255, 0, 0)) # Red LED + robot.audio_control("audio.wav") # Play WAV file + """ + # Set volume + self.audio_client.SetVolume(volume) + + # Detect command type and execute + if isinstance(command, tuple) and len(command) == 3: + # LED control - RGB tuple + r, g, b = command + logger_mp.info(f"Setting LED to RGB({r}, {g}, {b})") + self.audio_client.LedControl(r, g, b) + + elif isinstance(command, str): + # Check if it's a file path + if Path(command).exists(): + # Play WAV file + logger_mp.info(f"Playing audio file: {command}") + self._play_wav_file(command) + else: + # Text-to-speech + logger_mp.info(f"Speaking: {command}") + self.audio_client.TtsMaker(command, 0) # 0 for English + else: + raise ValueError( + f"Invalid command type: {type(command)}. " + "Expected str (text/path) or tuple[int, int, int] (RGB)" + ) + + def _read_wav_file(self, filename: str): + """Read WAV file and return PCM data as bytes.""" + with open(filename, 'rb') as f: + def read(fmt): + return struct.unpack(fmt, f.read(struct.calcsize(fmt))) + + # Read RIFF header + chunk_id, = read(' dict[str, Any]: obs_array = self.get_current_dual_arm_q() obs_dict = {f"{G1_29_JointArmIndex(motor).name}.pos": val for motor, val in zip(G1_29_JointArmIndex, obs_array, strict=True)} @@ -376,12 +615,16 @@ class UnitreeG1(Robot): #then teleop them wiuth the glove hehe #then we get ALL THE DATA if self.is_calibrated: + uncalibrated_action = action.copy() action = self.invert_calibration(action) + #if an action was 0.5 write 0 in its place + for key, value in uncalibrated_action.items(): + if value == 0.5: + action[key] = 0.0 #check if action is within bounds for key, value in action.items(): if value < self.calibration[key]["range_min"] or value > self.calibration[key]["range_max"]: raise ValueError(f"Action value {value} for {key} is out of bounds, actions are not normalized") - if self.freeze_body: arm_joint_indices = set(range(15, 29)) # 15–28 are arms for jid in G1_29_JointIndex: @@ -390,14 +633,17 @@ class UnitreeG1(Robot): self.msg.motor_cmd[jid].q = 0.0 self.msg.motor_cmd[jid].dq = 0.0 self.msg.motor_cmd[jid].tau = 0.0 - #print(action) + action_np = np.stack([v for v in action.values()]) + #action_np is just zeros + #action_np = np.zeros(14) #print(action_np) #exit() if self.gravity_compensation: tau = self.arm_ik.solve_tau(action_np) else: - tau = None + tau = np.zeros(14) + self.ctrl_dual_arm(action_np, tau) def apply_calibration(self, action: dict[str, float]) -> dict[str, float]: @@ -415,7 +661,8 @@ class UnitreeG1(Robot): norm = (value - mn) / (mx - mn) norm = max(0.0, min(1.0, norm)) - calibrated[key] = norm + # Round to 5 decimal places to avoid floating point precision issues + calibrated[key] = round(norm, 5) return calibrated @@ -431,7 +678,9 @@ class UnitreeG1(Robot): # inverse mapping real_val = mn + value * (mx - mn) - calibrated[key] = real_val + + # Round to 5 decimal places to avoid floating point precision issues + calibrated[key] = round(real_val, 5) return calibrated diff --git a/src/lerobot/robots/unitree_g1/utils/constants.py b/src/lerobot/robots/unitree_g1/utils/constants.py deleted file mode 100644 index 5d14f3886..000000000 --- a/src/lerobot/robots/unitree_g1/utils/constants.py +++ /dev/null @@ -1,280 +0,0 @@ -import dataclasses -from typing import List, Dict - - -@dataclasses.dataclass(frozen=True) -class RobotConfig: - motors: List[str] - cameras: List[str] - camera_to_image_key: Dict[str, str] - json_state_data_name: List[str] - json_action_data_name: List[str] - - -Z1_CONFIG = RobotConfig( - motors=[ - "kLeftWaist", - "kLeftShoulder", - "kLeftElbow", - "kLeftForearmRoll", - "kLeftWristAngle", - "kLeftWristRotate", - "kLeftGripper", - "kRightWaist", - "kRightShoulder", - "kRightElbow", - "kRightForearmRoll", - "kRightWristAngle", - "kRightWristRotate", - "kRightGripper", - ], - cameras=[ - "cam_high", - "cam_left_wrist", - "cam_right_wrist", - ], - camera_to_image_key={"color_0": "cam_high", "color_1": "cam_left_wrist", "color_2": "cam_right_wrist"}, - json_state_data_name=["left_arm", "right_arm"], - json_action_data_name=["left_arm", "right_arm"], -) - - -Z1_SINGLE_CONFIG = RobotConfig( - motors=[ - "kWaist", - "kShoulder", - "kElbow", - "kForearmRoll", - "kWristAngle", - "kWristRotate", - "kGripper", - ], - cameras=[ - "cam_high", - "cam_wrist", - ], - camera_to_image_key={"color_0": "cam_high", "color_1": "cam_wrist"}, - json_state_data_name=["left_arm", "right_arm"], - json_action_data_name=["left_arm", "right_arm"], -) - - -G1_DEX1_CONFIG = RobotConfig( - motors=[ - "kLeftShoulderPitch", - "kLeftShoulderRoll", - "kLeftShoulderYaw", - "kLeftElbow", - "kLeftWristRoll", - "kLeftWristPitch", - "kLeftWristYaw", - "kRightShoulderPitch", - "kRightShoulderRoll", - "kRightShoulderYaw", - "kRightElbow", - "kRightWristRoll", - "kRightWristPitch", - "kRightWristYaw", - "kLeftGripper", - "kRightGripper", - ], - cameras=[ - "cam_left_high", - "cam_right_high", - "cam_left_wrist", - "cam_right_wrist", - ], - camera_to_image_key={ - "color_0": "cam_left_high", - "color_1": "cam_right_high", - "color_2": "cam_left_wrist", - "color_3": "cam_right_wrist", - }, - json_state_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], - json_action_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], -) - - -G1_DEX1_CONFIG_SIM = RobotConfig( - motors=[ - "kLeftShoulderPitch", - "kLeftShoulderRoll", - "kLeftShoulderYaw", - "kLeftElbow", - "kLeftWristRoll", - "kLeftWristPitch", - "kLeftWristYaw", - "kRightShoulderPitch", - "kRightShoulderRoll", - "kRightShoulderYaw", - "kRightElbow", - "kRightWristRoll", - "kRightWristPitch", - "kRightWristYaw", - "kLeftGripper", - "kRightGripper", - ], - cameras=[ - "cam_left_high", - "cam_left_wrist", - "cam_right_wrist", - ], - camera_to_image_key={ - "color_0": "cam_left_high", - "color_1": "cam_left_wrist", - "color_2": "cam_right_wrist", - }, - json_state_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], - json_action_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], -) - - -G1_DEX3_CONFIG = RobotConfig( - motors=[ - "kLeftShoulderPitch", - "kLeftShoulderRoll", - "kLeftShoulderYaw", - "kLeftElbow", - "kLeftWristRoll", - "kLeftWristPitch", - "kLeftWristYaw", - "kRightShoulderPitch", - "kRightShoulderRoll", - "kRightShoulderYaw", - "kRightElbow", - "kRightWristRoll", - "kRightWristPitch", - "kRightWristYaw", - "kLeftHandThumb0", - "kLeftHandThumb1", - "kLeftHandThumb2", - "kLeftHandMiddle0", - "kLeftHandMiddle1", - "kLeftHandIndex0", - "kLeftHandIndex1", - "kRightHandThumb0", - "kRightHandThumb1", - "kRightHandThumb2", - "kRightHandIndex0", - "kRightHandIndex1", - "kRightHandMiddle0", - "kRightHandMiddle1", - ], - cameras=[ - "cam_left_high", - "cam_right_high", - "cam_left_wrist", - "cam_right_wrist", - ], - camera_to_image_key={ - "color_0": "cam_left_high", - "color_1": "cam_right_high", - "color_2": "cam_left_wrist", - "color_3": "cam_right_wrist", - }, - json_state_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], - json_action_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], -) - - -G1_BRAINCO_CONFIG = RobotConfig( - motors=[ - "kLeftShoulderPitch", - "kLeftShoulderRoll", - "kLeftShoulderYaw", - "kLeftElbow", - "kLeftWristRoll", - "kLeftWristPitch", - "kLeftWristYaw", - "kRightShoulderPitch", - "kRightShoulderRoll", - "kRightShoulderYaw", - "kRightElbow", - "kRightWristRoll", - "kRightWristPitch", - "kRightWristYaw", - "kLeftHandThumb", - "kLeftHandThumbAux", - "kLeftHandIndex", - "kLeftHandMiddle", - "kLeftHandRing", - "kLeftHandPinky", - "kRightHandThumb", - "kRightHandThumbAux", - "kRightHandIndex", - "kRightHandMiddle", - "kRightHandRing", - "kRightHandPinky", - ], - cameras=[ - "cam_left_high", - "cam_right_high", - "cam_left_wrist", - "cam_right_wrist", - ], - camera_to_image_key={ - "color_0": "cam_left_high", - "color_1": "cam_right_high", - "color_2": "cam_left_wrist", - "color_3": "cam_right_wrist", - }, - json_state_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], - json_action_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], -) - - -G1_INSPIRE_CONFIG = RobotConfig( - motors=[ - "kLeftShoulderPitch", - "kLeftShoulderRoll", - "kLeftShoulderYaw", - "kLeftElbow", - "kLeftWristRoll", - "kLeftWristPitch", - "kLeftWristYaw", - "kRightShoulderPitch", - "kRightShoulderRoll", - "kRightShoulderYaw", - "kRightElbow", - "kRightWristRoll", - "kRightWristPitch", - "kRightWristYaw", - "kLeftHandPinky", - "kLeftHandRing", - "kLeftHandMiddle", - "kLeftHandIndex", - "kLeftHandThumbBend", - "kLeftHandThumbRotation", - "kRightHandPinky", - "kRightHandRing", - "kRightHandMiddle", - "kRightHandIndex", - "kRightHandThumbBend", - "kRightHandThumbRotation", - ], - cameras=[ - "cam_left_high", - "cam_right_high", - "cam_left_wrist", - "cam_right_wrist", - ], - camera_to_image_key={ - "color_0": "cam_left_high", - "color_1": "cam_right_high", - "color_2": "cam_left_wrist", - "color_3": "cam_right_wrist", - }, - json_state_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], - json_action_data_name=["left_arm", "right_arm", "left_ee", "right_ee"], -) - - -ROBOT_CONFIGS = { - "Unitree_Z1_Single": Z1_SINGLE_CONFIG, - "Unitree_Z1_Dual": Z1_CONFIG, - "Unitree_G1_Dex1": G1_DEX1_CONFIG, - "Unitree_G1_Dex1_Sim": G1_DEX1_CONFIG_SIM, - "Unitree_G1_Dex3": G1_DEX3_CONFIG, - "Unitree_G1_Brainco": G1_BRAINCO_CONFIG, - "Unitree_G1_Inspire": G1_INSPIRE_CONFIG, -} diff --git a/src/lerobot/robots/unitree_g1/utils/convert_lerobot_to_h5.py b/src/lerobot/robots/unitree_g1/utils/convert_lerobot_to_h5.py deleted file mode 100644 index ac1b0df4c..000000000 --- a/src/lerobot/robots/unitree_g1/utils/convert_lerobot_to_h5.py +++ /dev/null @@ -1,161 +0,0 @@ -""" -Script lerobot to h5. -# --repo-id Your unique repo ID on Hugging Face Hub -# --output_dir Save path to h5 file - -python unitree_lerobot/utils/convert_lerobot_to_h5.py.py \ - --repo-id your_name/g1_grabcube_double_hand \ - --output_dir "$HOME/datasets/g1_grabcube_double_hand" -""" - -import os -import cv2 -import h5py -import tyro -import numpy as np -from tqdm import tqdm -from pathlib import Path -from collections import defaultdict -from lerobot.datasets.lerobot_dataset import LeRobotDataset - - -class LeRobotDataProcessor: - def __init__(self, repo_id: str, root: str = None, image_dtype: str = "to_unit8") -> None: - self.image_dtype = image_dtype - self.dataset = LeRobotDataset(repo_id=repo_id, root=root) - - def process_episode(self, episode_index: int) -> dict: - """Process a single episode to extract camera images, state, and action.""" - from_idx = self.dataset.episode_data_index["from"][episode_index].item() - to_idx = self.dataset.episode_data_index["to"][episode_index].item() - - episode = defaultdict(list) - cameras = defaultdict(list) - - for step_idx in tqdm( - range(from_idx, to_idx), desc=f"Episode {episode_index}", position=1, leave=False, dynamic_ncols=True - ): - step = self.dataset[step_idx] - - image_dict = { - key.split(".")[2]: cv2.cvtColor( - np.transpose((value.numpy() * 255).astype(np.uint8), (1, 2, 0)), cv2.COLOR_BGR2RGB - ) - for key, value in step.items() - if key.startswith("observation.image") and len(key.split(".")) >= 3 - } - - for key, value in image_dict.items(): - if self.image_dtype == "to_unit8": - cameras[key].append(value) - elif self.image_dtype == "to_bytes": - success, encoded_img = cv2.imencode(".jpg", value, [cv2.IMWRITE_JPEG_QUALITY, 100]) - if not success: - raise ValueError(f"Image encoding failed for key: {key}") - cameras[key].append(np.void(encoded_img.tobytes())) - - cam_height, cam_width = next(iter(image_dict.values())).shape[:2] - episode["state"].append(step["observation.state"]) - episode["action"].append(step["action"]) - - episode["cameras"] = cameras - episode["task"] = step["task"] - episode["episode_length"] = to_idx - from_idx - - # Data configuration for later use - episode["data_cfg"] = { - "camera_names": list(image_dict.keys()), - "cam_height": cam_height, - "cam_width": cam_width, - "state_dim": np.squeeze(step["observation.state"].numpy().shape), - "action_dim": np.squeeze(step["action"].numpy().shape), - } - episode["episode_index"] = episode_index - - return episode - - -class H5Writer: - def __init__(self, output_dir: Path) -> None: - self.output_dir = output_dir - os.makedirs(output_dir, exist_ok=True) - - def write_to_h5(self, episode: dict) -> None: - """Write episode data to HDF5 file.""" - - episode_length = episode["episode_length"] - episode_index = episode["episode_index"] - state = episode["state"] - action = episode["action"] - qvel = np.zeros_like(episode["state"]) - cameras = episode["cameras"] - task = episode["task"] - data_cfg = episode["data_cfg"] - - # Prepare data dictionary - data_dict = { - "/observations/qpos": [state], - "/observations/qvel": [qvel], - "/action": [action], - **{f"/observations/images/{k}": [v] for k, v in cameras.items()}, - } - - h5_path = os.path.join(self.output_dir, f"episode_{episode_index}.hdf5") - - with h5py.File(h5_path, "w", rdcc_nbytes=1024**2 * 2, libver="latest") as root: - # Set attributes - root.attrs["sim"] = False - - # Create datasets - obs = root.create_group("observations") - image = obs.create_group("images") - - # Write camera images - for cam_name, images in cameras.items(): - data_dtype = images[0].dtype - shape = ( - (episode_length, data_cfg["cam_height"], data_cfg["cam_width"], 3) - if data_dtype == "uint8" - else (episode_length,) - ) - chunks = (1, data_cfg["cam_height"], data_cfg["cam_width"], 3) if data_dtype == "uint8" else (1,) - image.create_dataset(cam_name, shape=shape, dtype=data_dtype, chunks=chunks, compression="gzip") - # root[f'/observations/images/{cam_name}'][...] = images - - # Write state and action data - obs.create_dataset("qpos", (episode_length, data_cfg["state_dim"]), dtype="float32", compression="gzip") - obs.create_dataset("qvel", (episode_length, data_cfg["state_dim"]), dtype="float32", compression="gzip") - root.create_dataset("action", (episode_length, data_cfg["action_dim"]), dtype="float32", compression="gzip") - - # Write metadata - root.create_dataset("is_edited", (1,), dtype="uint8") - substep_reasonings = root.create_dataset( - "substep_reasonings", (episode_length,), dtype=h5py.string_dtype(encoding="utf-8"), compression="gzip" - ) - root.create_dataset("language_raw", data=task) - substep_reasonings[:] = [task] * episode_length - - # Write additional data - for name, array in data_dict.items(): - root[name][...] = array - - -def lerobot_to_h5(repo_id: str, output_dir: Path, root: str = None) -> None: - """Main function to process and write LeRobot data to HDF5 format.""" - - # Initialize data processor and H5 writer - data_processor = LeRobotDataProcessor( - repo_id, root, image_dtype="to_unit8" - ) # image_dtype Options: "to_unit8", "to_bytes" - h5_writer = H5Writer(output_dir) - - # Process each episode - for episode_index in tqdm( - range(data_processor.dataset.num_episodes), desc="Episodes", position=0, dynamic_ncols=True - ): - episode = data_processor.process_episode(episode_index) - h5_writer.write_to_h5(episode) - - -if __name__ == "__main__": - tyro.cli(lerobot_to_h5) diff --git a/src/lerobot/robots/unitree_g1/utils/convert_unitree_json_to_h5.py b/src/lerobot/robots/unitree_g1/utils/convert_unitree_json_to_h5.py deleted file mode 100644 index d848f4dce..000000000 --- a/src/lerobot/robots/unitree_g1/utils/convert_unitree_json_to_h5.py +++ /dev/null @@ -1,250 +0,0 @@ -""" -Script Json to h5. - -# --data_dirs Corresponds to the directory of your JSON dataset -# --output_dir Save path to h5 file -# --robot_type The type of the robot used in the dataset (e.g., Unitree_Z1_Single, Unitree_Z1_Dual, Unitree_G1_Dex1, Unitree_G1_Dex3, Unitree_G1_Brainco, Unitree_G1_Inspire) - -python unitree_lerobot/utils/convert_unitree_json_to_h5.py \ - --data_dirs $HOME/datasets/json \ - --output_dir $HOME/datasets/h5 \ - --robot_type Unitree_G1_Dex3 -""" - -import os -import tyro -import json -import h5py -import cv2 -import tqdm -import glob -import numpy as np -from pathlib import Path -from typing import List, Dict, Optional -from collections import defaultdict -from unitree_lerobot.utils.constants import ROBOT_CONFIGS - - -class JsonDataset: - def __init__(self, data_dirs: Path, robot_type: str) -> None: - """ - Initialize the dataset for loading and processing HDF5 files containing robot manipulation data. - - Args: - data_dirs: Path to directory containing training data - """ - assert data_dirs is not None, "Data directory cannot be None" - assert robot_type is not None, "Robot type cannot be None" - self.data_dirs = data_dirs - self.json_file = "data.json" - - # Initialize paths and cache - self._init_paths() - self._init_cache() - self.json_state_data_name = ROBOT_CONFIGS[robot_type].json_state_data_name - self.json_action_data_name = ROBOT_CONFIGS[robot_type].json_action_data_name - self.camera_to_image_key = ROBOT_CONFIGS[robot_type].camera_to_image_key - - def _init_paths(self) -> None: - """Initialize episode and task paths.""" - - self.episode_paths = [] - self.task_paths = [] - - for task_path in glob.glob(os.path.join(self.data_dirs, "*")): - if os.path.isdir(task_path): - episode_paths = glob.glob(os.path.join(task_path, "*")) - if episode_paths: - self.task_paths.append(task_path) - self.episode_paths.extend(episode_paths) - - self.episode_paths = sorted(self.episode_paths) - self.episode_ids = list(range(len(self.episode_paths))) - - def __len__(self) -> int: - """Return the number of episodes in the dataset.""" - return len(self.episode_paths) - - def _init_cache(self) -> List: - """Initialize data cache if enabled.""" - - self.episodes_data_cached = [] - for episode_path in tqdm.tqdm(self.episode_paths, desc="Loading Cache Json"): - json_path = os.path.join(episode_path, self.json_file) - with open(json_path, "r", encoding="utf-8") as jsonf: - self.episodes_data_cached.append(json.load(jsonf)) - - print(f"==> Cached {len(self.episodes_data_cached)} episodes") - - return self.episodes_data_cached - - def _extract_data(self, episode_data: Dict, key: str, parts: List[str]) -> np.ndarray: - """ - Extract data from episode dictionary for specified parts. - - Args: - episode_data: Dictionary containing episode data - key: Data key to extract ('states' or 'actions') - parts: List of parts to include ('left_arm', 'right_arm') - - Returns: - Concatenated numpy array of the requested data - """ - result = [] - for sample_data in episode_data["data"]: - data_array = np.array([], dtype=np.float32) - for part in parts: - if part in sample_data[key] and sample_data[key][part] is not None: - qpos = np.array(sample_data[key][part]["qpos"], dtype=np.float32) - data_array = np.concatenate([data_array, qpos]) - result.append(data_array) - return np.array(result) - - def _parse_images(self, episode_path: str, episode_data) -> dict[str, list[np.ndarray]]: - """Load and stack images for a given camera key.""" - - images = defaultdict(list) - - keys = episode_data["data"][0]["colors"].keys() - cameras = [key for key in keys if "depth" not in key] - - for camera in cameras: - image_key = self.camera_to_image_key.get(camera) - - for sample_data in episode_data["data"]: - image_path = os.path.join(episode_path, sample_data["colors"].get(camera)) - if not os.path.exists(image_path): - raise FileNotFoundError(f"Image path does not exist: {image_path}") - - image = cv2.imread(image_path) - if image is None: - raise RuntimeError(f"Failed to read image: {image_path}") - - image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - images[image_key].append(image_rgb) - - return images - - def get_item( - self, - index: Optional[int] = None, - ) -> Dict: - """Get a training sample from the dataset.""" - - file_path = np.random.choice(self.episode_paths) if index is None else self.episode_paths[index] - episode_data = self.episodes_data_cached[index] - - # Load state and action data - action = self._extract_data(episode_data, "actions", self.json_action_data_name) - state = self._extract_data(episode_data, "states", self.json_state_data_name) - episode_length = len(state) - state_dim = state.shape[1] if len(state.shape) == 2 else state.shape[0] - action_dim = action.shape[1] if len(action.shape) == 2 else state.shape[0] - - # Load task description - task = episode_data.get("text", {}).get("goal", "") - - # Load camera images - cameras = self._parse_images(file_path, episode_data) - - # Extract camera configuration - cam_height, cam_width = next(img for imgs in cameras.values() if imgs for img in imgs).shape[:2] - data_cfg = { - "camera_names": list(cameras.keys()), - "cam_height": cam_height, - "cam_width": cam_width, - "state_dim": state_dim, - "action_dim": action_dim, - } - - return { - "episode_index": index, - "episode_length": episode_length, - "state": state, - "action": action, - "cameras": cameras, - "task": task, - "data_cfg": data_cfg, - } - - -class H5Writer: - def __init__(self, output_dir: Path) -> None: - self.output_dir = output_dir - os.makedirs(output_dir, exist_ok=True) - - def write_to_h5(self, episode: dict) -> None: - """Write episode data to HDF5 file.""" - - episode_length = episode["episode_length"] - episode_index = episode["episode_index"] - state = episode["state"] - action = episode["action"] - qvel = np.zeros_like(episode["state"]) - cameras = episode["cameras"] - task = episode["task"] - data_cfg = episode["data_cfg"] - - # Prepare data dictionary - data_dict = { - "/observations/qpos": [state], - "/observations/qvel": [qvel], - "/action": [action], - **{f"/observations/images/{k}": [v] for k, v in cameras.items()}, - } - - h5_path = os.path.join(self.output_dir, f"episode_{episode_index}.hdf5") - - with h5py.File(h5_path, "w", rdcc_nbytes=1024**2 * 2, libver="latest") as root: - # Set attributes - root.attrs["sim"] = False - - # Create datasets - obs = root.create_group("observations") - image = obs.create_group("images") - - # Write camera images - for cam_name, images in cameras.items(): - image.create_dataset( - cam_name, - shape=(episode_length, data_cfg["cam_height"], data_cfg["cam_width"], 3), - dtype="uint8", - chunks=(1, data_cfg["cam_height"], data_cfg["cam_width"], 3), - compression="gzip", - ) - # root[f'/observations/images/{cam_name}'][...] = images - - # Write state and action data - obs.create_dataset("qpos", (episode_length, data_cfg["state_dim"]), dtype="float32", compression="gzip") - obs.create_dataset("qvel", (episode_length, data_cfg["state_dim"]), dtype="float32", compression="gzip") - root.create_dataset("action", (episode_length, data_cfg["action_dim"]), dtype="float32", compression="gzip") - - # Write metadata - root.create_dataset("is_edited", (1,), dtype="uint8") - substep_reasonings = root.create_dataset( - "substep_reasonings", (episode_length,), dtype=h5py.string_dtype(encoding="utf-8"), compression="gzip" - ) - root.create_dataset("language_raw", data=task) - substep_reasonings[:] = [task] * episode_length - - # Write additional data - for name, array in data_dict.items(): - root[name][...] = array - - -def json_to_h5( - data_dirs: Path, - output_dir: Path, - robot_type: str, -) -> None: - """Convert JSON episode data to HDF5 format.""" - dataset = JsonDataset(data_dirs, robot_type) - h5_writer = H5Writer(output_dir) - - for i in tqdm.tqdm(range(len(dataset))): - episode = dataset.get_item(i) - h5_writer.write_to_h5(episode) - - -if __name__ == "__main__": - tyro.cli(json_to_h5) diff --git a/src/lerobot/robots/unitree_g1/utils/convert_unitree_json_to_lerobot.py b/src/lerobot/robots/unitree_g1/utils/convert_unitree_json_to_lerobot.py deleted file mode 100644 index 3f7ed607c..000000000 --- a/src/lerobot/robots/unitree_g1/utils/convert_unitree_json_to_lerobot.py +++ /dev/null @@ -1,341 +0,0 @@ -""" -Script Json to Lerobot. - -# --raw-dir Corresponds to the directory of your JSON dataset -# --repo-id Your unique repo ID on Hugging Face Hub -# --robot_type The type of the robot used in the dataset (e.g., Unitree_Z1_Single, Unitree_Z1_Dual, Unitree_G1_Dex1, Unitree_G1_Dex3, Unitree_G1_Brainco, Unitree_G1_Inspire) -# --push_to_hub Whether or not to upload the dataset to Hugging Face Hub (true or false) - -python unitree_lerobot/utils/convert_unitree_json_to_lerobot.py \ - --raw-dir $HOME/datasets/g1_grabcube_double_hand \ - --repo-id your_name/g1_grabcube_double_hand \ - --robot_type Unitree_G1_Dex3 \ - --push_to_hub -""" - -import os -import cv2 -import tqdm -import tyro -import json -import glob -import dataclasses -import shutil -import numpy as np -from pathlib import Path -from collections import defaultdict -from typing import Literal, List, Dict, Optional - -from lerobot.constants import HF_LEROBOT_HOME -from lerobot.datasets.lerobot_dataset import LeRobotDataset - -from unitree_lerobot.utils.constants import ROBOT_CONFIGS - - -@dataclasses.dataclass(frozen=True) -class DatasetConfig: - use_videos: bool = True - tolerance_s: float = 0.0001 - image_writer_processes: int = 10 - image_writer_threads: int = 5 - video_backend: str | None = None - - -DEFAULT_DATASET_CONFIG = DatasetConfig() - - -class JsonDataset: - def __init__(self, data_dirs: Path, robot_type: str) -> None: - """ - Initialize the dataset for loading and processing HDF5 files containing robot manipulation data. - - Args: - data_dirs: Path to directory containing training data - """ - assert data_dirs is not None, "Data directory cannot be None" - assert robot_type is not None, "Robot type cannot be None" - self.data_dirs = data_dirs - self.json_file = "data.json" - - # Initialize paths and cache - self._init_paths() - self._init_cache() - self.json_state_data_name = ROBOT_CONFIGS[robot_type].json_state_data_name - self.json_action_data_name = ROBOT_CONFIGS[robot_type].json_action_data_name - self.camera_to_image_key = ROBOT_CONFIGS[robot_type].camera_to_image_key - - def _init_paths(self) -> None: - """Initialize episode and task paths.""" - - self.episode_paths = [] - self.task_paths = [] - - for task_path in glob.glob(os.path.join(self.data_dirs, "*")): - if os.path.isdir(task_path): - episode_paths = glob.glob(os.path.join(task_path, "*")) - if episode_paths: - self.task_paths.append(task_path) - self.episode_paths.extend(episode_paths) - - self.episode_paths = sorted(self.episode_paths) - self.episode_ids = list(range(len(self.episode_paths))) - - def __len__(self) -> int: - """Return the number of episodes in the dataset.""" - return len(self.episode_paths) - - def _init_cache(self) -> List: - """Initialize data cache if enabled.""" - - self.episodes_data_cached = [] - for episode_path in tqdm.tqdm(self.episode_paths, desc="Loading Cache Json"): - json_path = os.path.join(episode_path, self.json_file) - with open(json_path, "r", encoding="utf-8") as jsonf: - self.episodes_data_cached.append(json.load(jsonf)) - - print(f"==> Cached {len(self.episodes_data_cached)} episodes") - - return self.episodes_data_cached - - def _extract_data(self, episode_data: Dict, key: str, parts: List[str]) -> np.ndarray: - """ - Extract data from episode dictionary for specified parts. - - Args: - episode_data: Dictionary containing episode data - key: Data key to extract ('states' or 'actions') - parts: List of parts to include ('left_arm', 'right_arm') - - Returns: - Concatenated numpy array of the requested data - """ - result = [] - for sample_data in episode_data["data"]: - data_array = np.array([], dtype=np.float32) - for part in parts: - if part in sample_data[key] and sample_data[key][part] is not None: - qpos = np.array(sample_data[key][part]["qpos"], dtype=np.float32) - data_array = np.concatenate([data_array, qpos]) - result.append(data_array) - return np.array(result) - - def _parse_images(self, episode_path: str, episode_data) -> dict[str, list[np.ndarray]]: - """Load and stack images for a given camera key.""" - - images = defaultdict(list) - - keys = episode_data["data"][0]["colors"].keys() - cameras = [key for key in keys if "depth" not in key] - - for camera in cameras: - image_key = self.camera_to_image_key.get(camera) - if image_key is None: - continue - - for sample_data in episode_data["data"]: - relative_path = sample_data["colors"].get(camera) - if not relative_path: - continue - - image_path = os.path.join(episode_path, relative_path) - if not os.path.exists(image_path): - raise FileNotFoundError(f"Image path does not exist: {image_path}") - - image = cv2.imread(image_path) - if image is None: - raise RuntimeError(f"Failed to read image: {image_path}") - - image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - images[image_key].append(image_rgb) - - return images - - def get_item( - self, - index: Optional[int] = None, - ) -> Dict: - """Get a training sample from the dataset.""" - - file_path = np.random.choice(self.episode_paths) if index is None else self.episode_paths[index] - episode_data = self.episodes_data_cached[index] - - # Load state and action data - action = self._extract_data(episode_data, "actions", self.json_action_data_name) - state = self._extract_data(episode_data, "states", self.json_state_data_name) - episode_length = len(state) - state_dim = state.shape[1] if len(state.shape) == 2 else state.shape[0] - action_dim = action.shape[1] if len(action.shape) == 2 else state.shape[0] - - # Load task description - task = episode_data.get("text", {}).get("goal", "") - - # Load camera images - cameras = self._parse_images(file_path, episode_data) - - # Extract camera configuration - cam_height, cam_width = next(img for imgs in cameras.values() if imgs for img in imgs).shape[:2] - data_cfg = { - "camera_names": list(cameras.keys()), - "cam_height": cam_height, - "cam_width": cam_width, - "state_dim": state_dim, - "action_dim": action_dim, - } - - return { - "episode_index": index, - "episode_length": episode_length, - "state": state, - "action": action, - "cameras": cameras, - "task": task, - "data_cfg": data_cfg, - } - - -def create_empty_dataset( - repo_id: str, - robot_type: str, - mode: Literal["video", "image"] = "video", - *, - has_velocity: bool = False, - has_effort: bool = False, - dataset_config: DatasetConfig = DEFAULT_DATASET_CONFIG, -) -> LeRobotDataset: - motors = ROBOT_CONFIGS[robot_type].motors - cameras = ROBOT_CONFIGS[robot_type].cameras - - features = { - "observation.state": { - "dtype": "float32", - "shape": (len(motors),), - "names": [ - motors, - ], - }, - "action": { - "dtype": "float32", - "shape": (len(motors),), - "names": [ - motors, - ], - }, - } - - if has_velocity: - features["observation.velocity"] = { - "dtype": "float32", - "shape": (len(motors),), - "names": [ - motors, - ], - } - - if has_effort: - features["observation.effort"] = { - "dtype": "float32", - "shape": (len(motors),), - "names": [ - motors, - ], - } - - for cam in cameras: - features[f"observation.images.{cam}"] = { - "dtype": mode, - "shape": (480, 640, 3), - "names": [ - "height", - "width", - "channel", - ], - } - - if Path(HF_LEROBOT_HOME / repo_id).exists(): - shutil.rmtree(HF_LEROBOT_HOME / repo_id) - - return LeRobotDataset.create( - repo_id=repo_id, - fps=30, - robot_type=robot_type, - features=features, - use_videos=dataset_config.use_videos, - tolerance_s=dataset_config.tolerance_s, - image_writer_processes=dataset_config.image_writer_processes, - image_writer_threads=dataset_config.image_writer_threads, - video_backend=dataset_config.video_backend, - ) - - -def populate_dataset( - dataset: LeRobotDataset, - raw_dir: Path, - robot_type: str, -) -> LeRobotDataset: - json_dataset = JsonDataset(raw_dir, robot_type) - for i in tqdm.tqdm(range(len(json_dataset))): - episode = json_dataset.get_item(i) - - state = episode["state"] - action = episode["action"] - cameras = episode["cameras"] - task = episode["task"] - episode_length = episode["episode_length"] - - num_frames = episode_length - for i in range(num_frames): - frame = { - "observation.state": state[i], - "action": action[i], - } - - for camera, img_array in cameras.items(): - frame[f"observation.images.{camera}"] = img_array[i] - - dataset.add_frame(frame, task=task) - - dataset.save_episode() - - return dataset - - -def json_to_lerobot( - raw_dir: Path, - repo_id: str, - robot_type: str, # e.g., Unitree_Z1_Single, Unitree_Z1_Dual, Unitree_G1_Dex1, Unitree_G1_Dex3, Unitree_G1_Brainco, Unitree_G1_Inspire - *, - push_to_hub: bool = False, - mode: Literal["video", "image"] = "video", - dataset_config: DatasetConfig = DEFAULT_DATASET_CONFIG, -): - if (HF_LEROBOT_HOME / repo_id).exists(): - shutil.rmtree(HF_LEROBOT_HOME / repo_id) - - dataset = create_empty_dataset( - repo_id, - robot_type=robot_type, - mode=mode, - has_effort=False, - has_velocity=False, - dataset_config=dataset_config, - ) - dataset = populate_dataset( - dataset, - raw_dir, - robot_type=robot_type, - ) - - if push_to_hub: - dataset.push_to_hub(upload_large_folder=True) - - -def local_push_to_hub( - repo_id: str, - root_path: Path, -): - dataset = LeRobotDataset(repo_id=repo_id, root=root_path) - dataset.push_to_hub(upload_large_folder=True) - - -if __name__ == "__main__": - tyro.cli(json_to_lerobot) diff --git a/src/lerobot/robots/unitree_g1/utils/sort_and_rename_folders.py b/src/lerobot/robots/unitree_g1/utils/sort_and_rename_folders.py deleted file mode 100644 index 6d018f59f..000000000 --- a/src/lerobot/robots/unitree_g1/utils/sort_and_rename_folders.py +++ /dev/null @@ -1,40 +0,0 @@ -""" -Script to convert Unitree json data to the LeRobot dataset v2.0 format. - -python unitree_lerobot/utils/sort_and_rename_folders.py --data_dir $HOME/datasets/g1_grabcube_double_hand -""" - -import os -import tyro -import uuid -from pathlib import Path - - -def sort_and_rename_folders(data_dir: Path) -> None: - # Get the list of folders sorted by name - folders = sorted([f for f in os.listdir(data_dir) if os.path.isdir(os.path.join(data_dir, f))]) - - temp_mapping = {} - - # First, rename all folders to unique temporary names - for folder in folders: - temp_name = str(uuid.uuid4()) - original_path = os.path.join(data_dir, folder) - temp_path = os.path.join(data_dir, temp_name) - os.rename(original_path, temp_path) - temp_mapping[temp_name] = folder - - # Then, rename them to the final target names - start_number = 0 - for temp_name, original_folder in temp_mapping.items(): - new_folder_name = f"episode_{start_number:04d}" - temp_path = os.path.join(data_dir, temp_name) - new_path = os.path.join(data_dir, new_folder_name) - os.rename(temp_path, new_path) - start_number += 1 - - print("The folders have been successfully renamed.") - - -if __name__ == "__main__": - tyro.cli(sort_and_rename_folders) diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index 0f247caef..07737cae3 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -51,6 +51,7 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, so100_leader, so101_leader, + custom, ) from lerobot.utils.import_utils import register_third_party_devices from lerobot.utils.utils import init_logging diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index 599489ec6..c873d22da 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -110,6 +110,7 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, so100_leader, so101_leader, + custom ) from lerobot.teleoperators.keyboard.teleop_keyboard import KeyboardTeleop from lerobot.utils.constants import ACTION, OBS_STR diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index 0a418f3bc..2a114297f 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -60,6 +60,7 @@ import rerun as rr from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 +from lerobot.cameras.zmq.configuration_zmq import ZMQCameraConfig # noqa: F401 from lerobot.configs import parser from lerobot.processor import ( RobotAction, @@ -76,6 +77,7 @@ from lerobot.robots import ( # noqa: F401 make_robot_from_config, so100_follower, so101_follower, + unitree_g1, ) from lerobot.teleoperators import ( # noqa: F401 Teleoperator, @@ -87,6 +89,7 @@ from lerobot.teleoperators import ( # noqa: F401 make_teleoperator_from_config, so100_leader, so101_leader, + custom ) from lerobot.utils.import_utils import register_third_party_devices from lerobot.utils.robot_utils import busy_wait diff --git a/src/lerobot/teleoperators/custom/__init__.py b/src/lerobot/teleoperators/custom/__init__.py new file mode 100644 index 000000000..b4ccbca3c --- /dev/null +++ b/src/lerobot/teleoperators/custom/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .config_custom import CustomConfig +from .custom import Custom \ No newline at end of file diff --git a/src/lerobot/teleoperators/custom/config_custom.py b/src/lerobot/teleoperators/custom/config_custom.py new file mode 100644 index 000000000..66b79c2d6 --- /dev/null +++ b/src/lerobot/teleoperators/custom/config_custom.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ..config import TeleoperatorConfig + + +@TeleoperatorConfig.register_subclass("custom") +@dataclass +class CustomConfig(TeleoperatorConfig): + """Custom teleoperator config that dynamically wraps a base teleoperator class. + + The base class and its configuration are loaded from a JSON config file at runtime. + Port and baud_rate are taken from the first device in the config file. + """ + config_path: str | None = None # REQUIRED: Path to custom config JSON file + port: str = "/dev/ttyACM0" # Default port + baud_rate: int = 115200 # Default baud rate diff --git a/src/lerobot/teleoperators/custom/custom.py b/src/lerobot/teleoperators/custom/custom.py new file mode 100644 index 000000000..f0c5f88ee --- /dev/null +++ b/src/lerobot/teleoperators/custom/custom.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import importlib +import json +import logging +from pathlib import Path + +from lerobot.motors.motors_bus import MotorNormMode + +from ..teleoperator import Teleoperator +from .config_custom import CustomConfig + +logger = logging.getLogger(__name__) + + +class Custom(Teleoperator): + """ + Custom teleoperator that dynamically wraps a base teleoperator class and applies configurable joint mapping. + The base class is specified in custom_config.json, allowing flexible teleoperator configurations. + """ + + config_class = CustomConfig + name = "custom" + + def __init__(self, config: CustomConfig): + # Load custom configuration from JSON file + if config.config_path is None: + raise ValueError( + "config_path must be provided for custom teleoperator. " + "Example: --teleop.config_path=/path/to/custom_config.json" + ) + + config_path = Path(config.config_path) + + with open(config_path) as f: + custom_config = json.load(f) + + logger.info(f"Loaded custom config from {config_path}") + logger.info(f"Found {len(custom_config)} teleoperator(s): {list(custom_config.keys())}") + + # Initialize the base Teleoperator class + super().__init__(config) + + # Store multiple base teleoperators and their action mappings + self.base_teleops = {} + self.robot_actions_configs = {} + + # Instantiate each base teleoperator from the config + for device_name, device_config in custom_config.items(): + base_class_name = device_config["base_class"] + + # Create a config copy for this teleoperator + from dataclasses import replace + teleop_config = replace( + config, + port=device_config.get("port", config.port), + id=device_config.get("id", f"{config.id}_{device_name}"), + baud_rate=device_config.get("baud_rate", config.baud_rate) + ) + + logger.info(f" {device_name}: class={base_class_name}, port={teleop_config.port}, id={teleop_config.id}") + + # Dynamically import and instantiate the base teleoperator class + module_path, class_name_full = base_class_name.rsplit(".", 1) + module = importlib.import_module(module_path) + base_class = getattr(module, class_name_full) + + # Store the teleoperator and its action mapping + self.base_teleops[device_name] = base_class(teleop_config) + self.robot_actions_configs[device_name] = device_config["robot_actions"] + + @property + def action_features(self) -> dict: + # Aggregate action features from all teleoperators' action mappings + all_actions = {} + for device_config in self.robot_actions_configs.values(): + for robot_action in device_config.keys(): + all_actions[robot_action] = float + return all_actions + + @property + def feedback_features(self) -> dict: + # Aggregate feedback features from all base teleoperators + all_feedback = {} + for teleop in self.base_teleops.values(): + all_feedback.update(teleop.feedback_features) + return all_feedback + + @property + def is_connected(self) -> bool: + # All teleoperators must be connected + return all(teleop.is_connected for teleop in self.base_teleops.values()) + + @property + def is_calibrated(self) -> bool: + # All teleoperators must be calibrated + return all(teleop.is_calibrated for teleop in self.base_teleops.values()) + + def connect(self, calibrate: bool = True) -> None: + # Connect all base teleoperators + for device_name, teleop in self.base_teleops.items(): + logger.info(f"Connecting {device_name}...") + teleop.connect(calibrate=calibrate) + + def calibrate(self) -> None: + # Calibrate all base teleoperators + for device_name, teleop in self.base_teleops.items(): + logger.info(f"Calibrating {device_name}...") + teleop.calibrate() + + def configure(self) -> None: + # Configure all base teleoperators + for teleop in self.base_teleops.values(): + teleop.configure() + + def send_feedback(self, feedback: dict[str, float]) -> None: + # Send feedback to all base teleoperators + for teleop in self.base_teleops.values(): + teleop.send_feedback(feedback) + + def disconnect(self) -> None: + # Disconnect all base teleoperators + for device_name, teleop in self.base_teleops.items(): + logger.info(f"Disconnecting {device_name}...") + teleop.disconnect() + + def _normalize_to_unit_range(self, teleop, joint_name: str, value: float) -> float: + """Convert a joint value from base teleoperator's normalization mode to [0, 1] range. + + Args: + teleop: The base teleoperator instance + joint_name: Name of the joint (e.g., "shoulder_pitch") + value: Value in the base teleoperator's normalization mode + + Returns: + Value normalized to [0, 1] range + """ + norm_mode = teleop.joints[joint_name] + + if norm_mode == MotorNormMode.RANGE_M100_100: + # Convert from [-100, 100] to [0, 1] + return (value + 100.0) / 200.0 + elif norm_mode == MotorNormMode.RANGE_0_100: + # Convert from [0, 100] to [0, 1] + return value / 100.0 + elif norm_mode == MotorNormMode.DEGREES: + # For degrees, we need calibration to know the range + # Use calibration min/max to normalize + if teleop.calibration and joint_name in teleop.calibration: + min_deg = teleop.calibration[joint_name].range_min + max_deg = teleop.calibration[joint_name].range_max + if max_deg != min_deg: + return (value - min_deg) / (max_deg - min_deg) + # Fallback: assume common range like [-180, 180] + return (value + 180.0) / 360.0 + else: + raise ValueError(f"Unknown normalization mode: {norm_mode}") + + def get_action(self) -> dict[str, float]: + # Build action dict by reading from all base teleoperators + action = {} + + # Loop through each teleoperator + for device_name, teleop in self.base_teleops.items(): + # Read joint positions from this teleoperator + # These are in the teleoperator's normalization mode (e.g., -100 to 100) + joint_positions = teleop._read() + + # Get the robot actions config for this teleoperator + robot_actions_config = self.robot_actions_configs[device_name] + + # Process each robot action for this teleoperator + for robot_action, config in robot_actions_config.items(): + if config["source"] == "neutral": + # Use fixed neutral value (already in [0, 1] range) + value = config["value"] + elif config["source"] == "teleop": + # Get value from teleop joint + teleop_joint = config["joint"] + value = joint_positions[teleop_joint] + + # Convert from base teleoperator's normalization mode to [0, 1] range + value = self._normalize_to_unit_range(teleop, teleop_joint, value) + + # Apply inversion if specified + if config.get("invert", False): + value = 1.0 - value + else: + raise ValueError(f"Unknown source '{config['source']}' for robot action '{robot_action}'") + + action[robot_action] = value + return action diff --git a/src/lerobot/teleoperators/homunculus/homunculus_arm.py b/src/lerobot/teleoperators/homunculus/homunculus_arm.py index 43116f5c0..c96d866cd 100644 --- a/src/lerobot/teleoperators/homunculus/homunculus_arm.py +++ b/src/lerobot/teleoperators/homunculus/homunculus_arm.py @@ -16,6 +16,7 @@ import logging import threading +import time from collections import deque from pprint import pformat