From c2fb6446131e15ffc0e748e44df1a34519f731ce Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Mon, 15 Dec 2025 23:50:29 +0900 Subject: [PATCH 1/2] feat(robot): Add support for OMX robot (#2614) * upload * feat(omx): simplify motor initialization and remove default calibration files * feat(omx): read motor positions without normalization for improved accuracy * update calibration method for return factory value Signed-off-by: Junha Cha * change the drive mode * refactor: clean up code by removing unnecessary blank lines in omx_follower and omx_leader modules * feat(omx): update calibration method to set drive modes for motors * feat(pyproject): add 'ROBOTIS' to extend-ignore-identifiers-re list * feat(omx): enhance calibration method to write default drive modes to motors * Update src/lerobot/robots/omx_follower/__init__.py Add informations about the robot Co-authored-by: Steven Palma Signed-off-by: Woojin Wie --------- Signed-off-by: Junha Cha Signed-off-by: Woojin Wie Co-authored-by: Junha02 Co-authored-by: Junha Cha Co-authored-by: Steven Palma --- pyproject.toml | 1 + src/lerobot/async_inference/constants.py | 2 +- src/lerobot/async_inference/robot_client.py | 1 + src/lerobot/robots/omx_follower/__init__.py | 21 ++ .../omx_follower/config_omx_follower.py | 39 +++ .../robots/omx_follower/omx_follower.py | 225 ++++++++++++++++++ src/lerobot/robots/utils.py | 4 + src/lerobot/scripts/lerobot_calibrate.py | 2 + .../scripts/lerobot_find_joint_limits.py | 2 + src/lerobot/scripts/lerobot_record.py | 9 +- src/lerobot/scripts/lerobot_replay.py | 1 + src/lerobot/scripts/lerobot_setup_motors.py | 4 + src/lerobot/scripts/lerobot_teleoperate.py | 2 + .../teleoperators/omx_leader/__init__.py | 18 ++ .../omx_leader/config_omx_leader.py | 30 +++ .../teleoperators/omx_leader/omx_leader.py | 165 +++++++++++++ src/lerobot/teleoperators/utils.py | 4 + 17 files changed, 528 insertions(+), 2 deletions(-) create mode 100644 src/lerobot/robots/omx_follower/__init__.py create mode 100644 src/lerobot/robots/omx_follower/config_omx_follower.py create mode 100644 src/lerobot/robots/omx_follower/omx_follower.py create mode 100644 src/lerobot/teleoperators/omx_leader/__init__.py create mode 100644 src/lerobot/teleoperators/omx_leader/config_omx_leader.py create mode 100644 src/lerobot/teleoperators/omx_leader/omx_leader.py diff --git a/pyproject.toml b/pyproject.toml index 050b604e8..9458c0127 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -263,6 +263,7 @@ default.extend-ignore-identifiers-re = [ "ein", "thw", "inpt", + "ROBOTIS", ] # TODO: Uncomment when ready to use diff --git a/src/lerobot/async_inference/constants.py b/src/lerobot/async_inference/constants.py index 1b1dac0f5..f8b6d7bb3 100644 --- a/src/lerobot/async_inference/constants.py +++ b/src/lerobot/async_inference/constants.py @@ -26,4 +26,4 @@ DEFAULT_OBS_QUEUE_TIMEOUT = 2 SUPPORTED_POLICIES = ["act", "smolvla", "diffusion", "tdmpc", "vqbet", "pi0", "pi05"] # TODO: Add all other robots -SUPPORTED_ROBOTS = ["so100_follower", "so101_follower", "bi_so100_follower"] +SUPPORTED_ROBOTS = ["so100_follower", "so101_follower", "bi_so100_follower", "omx_follower"] diff --git a/src/lerobot/async_inference/robot_client.py b/src/lerobot/async_inference/robot_client.py index f9d70a64e..d32aa6a21 100644 --- a/src/lerobot/async_inference/robot_client.py +++ b/src/lerobot/async_inference/robot_client.py @@ -54,6 +54,7 @@ from lerobot.robots import ( # noqa: F401 bi_so100_follower, koch_follower, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) diff --git a/src/lerobot/robots/omx_follower/__init__.py b/src/lerobot/robots/omx_follower/__init__.py new file mode 100644 index 000000000..db48dffe9 --- /dev/null +++ b/src/lerobot/robots/omx_follower/__init__.py @@ -0,0 +1,21 @@ +#!/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. + +# OMX is a fully open-source robot from ROBOTIS. +# More information at: https://ai.robotis.com/omx/introduction_omx.html + +from .config_omx_follower import OmxFollowerConfig +from .omx_follower import OmxFollower diff --git a/src/lerobot/robots/omx_follower/config_omx_follower.py b/src/lerobot/robots/omx_follower/config_omx_follower.py new file mode 100644 index 000000000..db4179fdf --- /dev/null +++ b/src/lerobot/robots/omx_follower/config_omx_follower.py @@ -0,0 +1,39 @@ +# Copyright 2024 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("omx_follower") +@dataclass +class OmxFollowerConfig(RobotConfig): + # Port to connect to the arm + port: str + + 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 + cameras: dict[str, CameraConfig] = field(default_factory=dict) + + # Set to `True` for backward compatibility with previous policies/dataset + use_degrees: bool = False diff --git a/src/lerobot/robots/omx_follower/omx_follower.py b/src/lerobot/robots/omx_follower/omx_follower.py new file mode 100644 index 000000000..2dd851377 --- /dev/null +++ b/src/lerobot/robots/omx_follower/omx_follower.py @@ -0,0 +1,225 @@ +#!/usr/bin/env python + +# Copyright 2024 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, MotorCalibration, MotorNormMode +from lerobot.motors.dynamixel import ( + DriveMode, + DynamixelMotorsBus, + OperatingMode, +) +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from ..robot import Robot +from ..utils import ensure_safe_goal_position +from .config_omx_follower import OmxFollowerConfig + +logger = logging.getLogger(__name__) + + +class OmxFollower(Robot): + """ + - [OMX](https://github.com/ROBOTIS-GIT/open_manipulator), + expansion, developed by Woojin Wie and Junha Cha from [ROBOTIS](https://ai.robotis.com/) + """ + + config_class = OmxFollowerConfig + name = "omx_follower" + + def __init__(self, config: OmxFollowerConfig): + super().__init__(config) + self.config = config + norm_mode_body = MotorNormMode.DEGREES if config.use_degrees else MotorNormMode.RANGE_M100_100 + self.bus = DynamixelMotorsBus( + port=self.config.port, + motors={ + "shoulder_pan": Motor(11, "xl430-w250", norm_mode_body), + "shoulder_lift": Motor(12, "xl430-w250", norm_mode_body), + "elbow_flex": Motor(13, "xl430-w250", norm_mode_body), + "wrist_flex": Motor(14, "xl330-m288", norm_mode_body), + "wrist_roll": Motor(15, "xl330-m288", norm_mode_body), + "gripper": Motor(16, "xl330-m288", MotorNormMode.RANGE_0_100), + }, + calibration=self.calibration, + ) + self.cameras = make_cameras_from_configs(config.cameras) + + @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: + """ + For OMX robots that come pre-calibrated: + - If default calibration from package doesn't match motors, read from motors and save + - This allows using pre-calibrated robots without manual calibration + - If no calibration file exists, use factory default values (homing_offset=0, range_min=0, range_max=4095) + """ + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + + self.bus.connect() + if not self.is_calibrated and calibrate: + logger.info( + "Mismatch between calibration values in the motor and the calibration file or no calibration file found" + ) + self.calibrate() + + 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: + self.bus.disable_torque() + logger.info(f"\nUsing factory default calibration values for {self}") + logger.info(f"\nWriting default configuration of {self} to the motors") + for motor in self.bus.motors: + self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value) + + for motor in self.bus.motors: + self.bus.write("Drive_Mode", motor, DriveMode.NON_INVERTED.value) + + self.calibration = {} + for motor, m in self.bus.motors.items(): + self.calibration[motor] = MotorCalibration( + id=m.id, + drive_mode=0, + homing_offset=0, + range_min=0, + range_max=4095, + ) + + self.bus.write_calibration(self.calibration) + self._save_calibration() + logger.info(f"Calibration saved to {self.calibration_fpath}") + + def configure(self) -> None: + with self.bus.torque_disabled(): + self.bus.configure_motors() + # Use 'extended position mode' for all motors except gripper, because in joint mode the servos + # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling + # the arm, you could end up with a servo with a position 0 or 4095 at a crucial point + for motor in self.bus.motors: + if motor != "gripper": + self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value) + + # Use 'position control current based' for gripper to be limited by the limit of the current. For + # the follower gripper, it means it can grasp an object without forcing too much even tho, its + # goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). + # For the leader gripper, it means we can use it as a physical trigger, since we can force with + # our finger to make it move, and it will move back to its original target position when we + # release the force. + self.bus.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) + + # Set better PID values to close the gap between recorded states and actions + # TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor + self.bus.write("Position_P_Gain", "elbow_flex", 1500) + self.bus.write("Position_I_Gain", "elbow_flex", 0) + self.bus.write("Position_D_Gain", "elbow_flex", 600) + + def setup_motors(self) -> None: + 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") + 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, float]) -> dict[str, float]: + """Command arm to move to a target joint configuration. + + The relative action magnitude may be clipped depending on the configuration parameter + `max_relative_target`. In this case, the action sent differs from original action. + Thus, this function always returns the action actually sent. + + Args: + action (dict[str, float]): The goal positions for the motors. + + Returns: + dict[str, float]: The action sent to the motors, potentially clipped. + """ + 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) + + # Send goal position to the arm + 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/utils.py b/src/lerobot/robots/utils.py index 4e8001538..9c5043335 100644 --- a/src/lerobot/robots/utils.py +++ b/src/lerobot/robots/utils.py @@ -28,6 +28,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot: from .koch_follower import KochFollower return KochFollower(config) + elif config.type == "omx_follower": + from .omx_follower import OmxFollower + + return OmxFollower(config) elif config.type == "so100_follower": from .so100_follower import SO100Follower diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index 8247ec053..910a9a1b5 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -40,6 +40,7 @@ from lerobot.robots import ( # noqa: F401 koch_follower, lekiwi, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) @@ -49,6 +50,7 @@ from lerobot.teleoperators import ( # noqa: F401 homunculus, koch_leader, make_teleoperator_from_config, + omx_leader, so100_leader, so101_leader, ) diff --git a/src/lerobot/scripts/lerobot_find_joint_limits.py b/src/lerobot/scripts/lerobot_find_joint_limits.py index 95fbd0646..f97c0d820 100644 --- a/src/lerobot/scripts/lerobot_find_joint_limits.py +++ b/src/lerobot/scripts/lerobot_find_joint_limits.py @@ -46,6 +46,7 @@ from lerobot.robots import ( # noqa: F401 RobotConfig, koch_follower, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) @@ -54,6 +55,7 @@ from lerobot.teleoperators import ( # noqa: F401 gamepad, koch_leader, make_teleoperator_from_config, + omx_leader, so100_leader, so101_leader, ) diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index 255e9681c..58aa6e16d 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -97,6 +97,7 @@ from lerobot.robots import ( # noqa: F401 hope_jr, koch_follower, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) @@ -107,6 +108,7 @@ from lerobot.teleoperators import ( # noqa: F401 homunculus, koch_leader, make_teleoperator_from_config, + omx_leader, so100_leader, so101_leader, ) @@ -270,7 +272,12 @@ def record_loop( for t in teleop if isinstance( t, - (so100_leader.SO100Leader | so101_leader.SO101Leader | koch_leader.KochLeader), + ( + so100_leader.SO100Leader + | so101_leader.SO101Leader + | koch_leader.KochLeader + | omx_leader.OmxLeader + ), ) ), None, diff --git a/src/lerobot/scripts/lerobot_replay.py b/src/lerobot/scripts/lerobot_replay.py index 52cb1d73c..d5808c768 100644 --- a/src/lerobot/scripts/lerobot_replay.py +++ b/src/lerobot/scripts/lerobot_replay.py @@ -58,6 +58,7 @@ from lerobot.robots import ( # noqa: F401 hope_jr, koch_follower, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) diff --git a/src/lerobot/scripts/lerobot_setup_motors.py b/src/lerobot/scripts/lerobot_setup_motors.py index c1d256c21..b721e55ca 100644 --- a/src/lerobot/scripts/lerobot_setup_motors.py +++ b/src/lerobot/scripts/lerobot_setup_motors.py @@ -33,6 +33,7 @@ from lerobot.robots import ( # noqa: F401 koch_follower, lekiwi, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) @@ -40,6 +41,7 @@ from lerobot.teleoperators import ( # noqa: F401 TeleoperatorConfig, koch_leader, make_teleoperator_from_config, + omx_leader, so100_leader, so101_leader, ) @@ -47,6 +49,8 @@ from lerobot.teleoperators import ( # noqa: F401 COMPATIBLE_DEVICES = [ "koch_follower", "koch_leader", + "omx_follower", + "omx_leader", "so100_follower", "so100_leader", "so101_follower", diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index d83766b67..bf722d6f1 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -75,6 +75,7 @@ from lerobot.robots import ( # noqa: F401 hope_jr, koch_follower, make_robot_from_config, + omx_follower, so100_follower, so101_follower, ) @@ -87,6 +88,7 @@ from lerobot.teleoperators import ( # noqa: F401 keyboard, koch_leader, make_teleoperator_from_config, + omx_leader, so100_leader, so101_leader, ) diff --git a/src/lerobot/teleoperators/omx_leader/__init__.py b/src/lerobot/teleoperators/omx_leader/__init__.py new file mode 100644 index 000000000..04d96d63e --- /dev/null +++ b/src/lerobot/teleoperators/omx_leader/__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_omx_leader import OmxLeaderConfig +from .omx_leader import OmxLeader diff --git a/src/lerobot/teleoperators/omx_leader/config_omx_leader.py b/src/lerobot/teleoperators/omx_leader/config_omx_leader.py new file mode 100644 index 000000000..3c0420ab2 --- /dev/null +++ b/src/lerobot/teleoperators/omx_leader/config_omx_leader.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +# Copyright 2024 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("omx_leader") +@dataclass +class OmxLeaderConfig(TeleoperatorConfig): + # Port to connect to the arm + port: str + + # Sets the arm in torque mode with the gripper motor set to this value. This makes it possible to squeeze + # the gripper and have it spring back to an open position on its own. + gripper_open_pos: float = 37.0 diff --git a/src/lerobot/teleoperators/omx_leader/omx_leader.py b/src/lerobot/teleoperators/omx_leader/omx_leader.py new file mode 100644 index 000000000..c0e49b558 --- /dev/null +++ b/src/lerobot/teleoperators/omx_leader/omx_leader.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python + +# Copyright 2024 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 lerobot.motors import Motor, MotorCalibration, MotorNormMode +from lerobot.motors.dynamixel import ( + DriveMode, + DynamixelMotorsBus, + OperatingMode, +) +from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError + +from ..teleoperator import Teleoperator +from .config_omx_leader import OmxLeaderConfig + +logger = logging.getLogger(__name__) + + +class OmxLeader(Teleoperator): + """ + - [OMX](https://github.com/ROBOTIS-GIT/open_manipulator), + expansion, developed by Woojin Wie and Junha Cha from [ROBOTIS](https://ai.robotis.com/) + """ + + config_class = OmxLeaderConfig + name = "omx_leader" + + def __init__(self, config: OmxLeaderConfig): + super().__init__(config) + self.config = config + self.bus = DynamixelMotorsBus( + port=self.config.port, + motors={ + "shoulder_pan": Motor(1, "xl330-m288", MotorNormMode.RANGE_M100_100), + "shoulder_lift": Motor(2, "xl330-m288", MotorNormMode.RANGE_M100_100), + "elbow_flex": Motor(3, "xl330-m288", MotorNormMode.RANGE_M100_100), + "wrist_flex": Motor(4, "xl330-m288", MotorNormMode.RANGE_M100_100), + "wrist_roll": Motor(5, "xl330-m288", MotorNormMode.RANGE_M100_100), + "gripper": Motor(6, "xl330-m077", MotorNormMode.RANGE_0_100), + }, + calibration=self.calibration, + ) + + @property + def action_features(self) -> dict[str, type]: + return {f"{motor}.pos": float for motor in self.bus.motors} + + @property + def feedback_features(self) -> dict[str, type]: + return {} + + @property + def is_connected(self) -> bool: + return self.bus.is_connected + + def connect(self, calibrate: bool = True) -> None: + if self.is_connected: + raise DeviceAlreadyConnectedError(f"{self} already connected") + + self.bus.connect() + if not self.is_calibrated and calibrate: + logger.info( + "Mismatch between calibration values in the motor and the calibration file or no calibration file found" + ) + self.calibrate() + + self.configure() + logger.info(f"{self} connected.") + + @property + def is_calibrated(self) -> bool: + return self.bus.is_calibrated + + def calibrate(self) -> None: + self.bus.disable_torque() + logger.info(f"\nUsing factory default calibration values for {self}") + logger.info(f"\nWriting default configuration of {self} to the motors") + for motor in self.bus.motors: + self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value) + + for motor in self.bus.motors: + if motor == "gripper": + self.bus.write("Drive_Mode", motor, DriveMode.INVERTED.value) + else: + self.bus.write("Drive_Mode", motor, DriveMode.NON_INVERTED.value) + drive_modes = {motor: 1 if motor == "gripper" else 0 for motor in self.bus.motors} + + self.calibration = {} + for motor, m in self.bus.motors.items(): + self.calibration[motor] = MotorCalibration( + id=m.id, + drive_mode=drive_modes[motor], + homing_offset=0, + range_min=0, + range_max=4095, + ) + + self.bus.write_calibration(self.calibration) + self._save_calibration() + logger.info(f"Calibration saved to {self.calibration_fpath}") + + def configure(self) -> None: + self.bus.disable_torque() + self.bus.configure_motors() + for motor in self.bus.motors: + if motor != "gripper": + # Use 'extended position mode' for all motors except gripper, because in joint mode the servos + # can't rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while + # assembling the arm, you could end up with a servo with a position 0 or 4095 at a crucial + # point + self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value) + + # Use 'position control current based' for gripper to be limited by the limit of the current. + # For the follower gripper, it means it can grasp an object without forcing too much even tho, + # its goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). + # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger + # to make it move, and it will move back to its original target position when we release the force. + self.bus.write("Operating_Mode", "gripper", OperatingMode.CURRENT_POSITION.value) + # Set gripper's goal pos in current position mode so that we can use it as a trigger. + self.bus.enable_torque("gripper") + if self.is_calibrated: + self.bus.write("Goal_Position", "gripper", self.config.gripper_open_pos) + + def setup_motors(self) -> None: + 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_action(self) -> dict[str, float]: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + start = time.perf_counter() + action = self.bus.sync_read("Present_Position") + action = {f"{motor}.pos": val for motor, val in action.items()} + dt_ms = (time.perf_counter() - start) * 1e3 + logger.debug(f"{self} read action: {dt_ms:.1f}ms") + return action + + def send_feedback(self, feedback: dict[str, float]) -> None: + # TODO(rcadene, aliberts): Implement force feedback + raise NotImplementedError + + def disconnect(self) -> None: + if not self.is_connected: + raise DeviceNotConnectedError(f"{self} is not connected.") + + self.bus.disconnect() + logger.info(f"{self} disconnected.") diff --git a/src/lerobot/teleoperators/utils.py b/src/lerobot/teleoperators/utils.py index 2103a1669..699d1253f 100644 --- a/src/lerobot/teleoperators/utils.py +++ b/src/lerobot/teleoperators/utils.py @@ -41,6 +41,10 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator: from .koch_leader import KochLeader return KochLeader(config) + elif config.type == "omx_leader": + from .omx_leader import OmxLeader + + return OmxLeader(config) elif config.type == "so100_leader": from .so100_leader import SO100Leader From a6c3a0fa09fbbb1dd9063fe62aad1d2c800f3903 Mon Sep 17 00:00:00 2001 From: Martino Russi <77496684+nepyope@users.noreply.github.com> Date: Mon, 15 Dec 2025 16:22:27 +0100 Subject: [PATCH 2/2] Feat/add mj env (#2613) * add sim support * close fix threading issues --- docs/source/unitree_g1.mdx | 7 +++- .../robots/unitree_g1/config_unitree_g1.py | 3 ++ .../robots/unitree_g1/run_g1_server.py | 32 ++++++++--------- src/lerobot/robots/unitree_g1/unitree_g1.py | 35 ++++++++++++++----- 4 files changed, 51 insertions(+), 26 deletions(-) diff --git a/docs/source/unitree_g1.mdx b/docs/source/unitree_g1.mdx index 8f91e7791..af06fd742 100644 --- a/docs/source/unitree_g1.mdx +++ b/docs/source/unitree_g1.mdx @@ -4,11 +4,12 @@ This guide covers the complete setup process for the Unitree G1 humanoid, from i ## About the Unitree G1 -We offer support for both 29 and 23 DOF G1. In this first PR we introduce: +We offer support for both 29 and 23 DOF G1. We introduce: - **`unitree g1` robot class, handling low level communication with the humanoid** - **ZMQ socket bridge** for remote communication over WiFi, allowing one to deploy policies remotely instead of over ethernet or directly on the Orin - **GR00T locomotion policy** for bipedal walking and balance +- **MuJoCo simulation mode** for testing policies without the physical robot --- @@ -191,6 +192,10 @@ Press `Ctrl+C` to stop the policy. --- +## Extra: Running in Simulation Mode (MuJoCo) + +You can now test and develop policies without a physical robot using MuJoCo. to do so set `is_simulation=True` in config. + ## Additional Resources - [Unitree SDK Documentation](https://github.com/unitreerobotics/unitree_sdk2_python) diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index 575ad50bb..ac65f1a7b 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -51,5 +51,8 @@ class UnitreeG1Config(RobotConfig): control_dt: float = 1.0 / 250.0 # 250Hz + # launch mujoco simulation + is_simulation: bool = True + # socket config for ZMQ bridge robot_ip: str = "192.168.123.164" diff --git a/src/lerobot/robots/unitree_g1/run_g1_server.py b/src/lerobot/robots/unitree_g1/run_g1_server.py index ee3505ea4..70166b590 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/src/lerobot/robots/unitree_g1/run_g1_server.py @@ -99,11 +99,12 @@ def state_forward_loop( lowstate_sub: ChannelSubscriber, lowstate_sock: zmq.Socket, state_period: float, + shutdown_event: threading.Event, ) -> None: """Read observation from DDS and forward to ZMQ clients.""" last_state_time = 0.0 - while True: + while not shutdown_event.is_set(): # read from DDS msg = lowstate_sub.Read() if msg is None: @@ -128,7 +129,10 @@ def cmd_forward_loop( ) -> None: """Receive commands from ZMQ and forward to DDS.""" while True: - payload = lowcmd_sock.recv() + try: + payload = lowcmd_sock.recv() + except zmq.ContextTerminated: + break msg_dict = json.loads(payload.decode("utf-8")) topic = msg_dict.get("topic", "") @@ -182,30 +186,26 @@ def main() -> None: lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}") state_period = 0.002 # ~500 hz + shutdown_event = threading.Event() - # start observation forwarding thread + # start observation forwarding in background thread t_state = threading.Thread( target=state_forward_loop, - args=(lowstate_sub, lowstate_sock, state_period), - daemon=True, + args=(lowstate_sub, lowstate_sock, state_period, shutdown_event), ) t_state.start() - # start action forwarding thread - t_cmd = threading.Thread( - target=cmd_forward_loop, - args=(lowcmd_sock, lowcmd_pub_debug, crc), - daemon=True, - ) - t_cmd.start() - print("bridge running (lowstate -> zmq, lowcmd -> dds)") - # keep main thread alive so daemon threads don't exit + + # run command forwarding in main thread try: - while True: - time.sleep(1.0) + cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc) except KeyboardInterrupt: print("shutting down bridge...") + finally: + shutdown_event.set() + ctx.term() # terminates blocking zmq.recv() calls + t_state.join(timeout=2.0) if __name__ == "__main__": diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index 2e7196b57..cce9d1b1e 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -30,12 +30,8 @@ from unitree_sdk2py.idl.unitree_hg.msg.dds_ import ( ) from unitree_sdk2py.utils.crc import CRC +from lerobot.envs.factory import make_env from lerobot.robots.unitree_g1.g1_utils import G1_29_JointIndex -from lerobot.robots.unitree_g1.unitree_sdk2_socket import ( - ChannelFactoryInitialize, - ChannelPublisher, - ChannelSubscriber, -) from ..robot import Robot from .config_unitree_g1 import UnitreeG1Config @@ -127,7 +123,21 @@ class UnitreeG1(Robot): self.control_dt = config.control_dt + if config.is_simulation: + from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, + ) + else: + from lerobot.robots.unitree_g1.unitree_sdk2_socket import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, + ) + # connect robot + self.ChannelFactoryInitialize = ChannelFactoryInitialize self.connect() # initialize direct motor control interface @@ -138,8 +148,8 @@ class UnitreeG1(Robot): self.lowstate_buffer = DataBuffer() # initialize subscribe thread to read robot state + self._shutdown_event = threading.Event() self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state) - self.subscribe_thread.daemon = True self.subscribe_thread.start() while not self.is_connected: @@ -174,7 +184,7 @@ class UnitreeG1(Robot): self.remote_controller = self.RemoteController() def _subscribe_motor_state(self): # polls robot state @ 250Hz - while True: + while not self._shutdown_event.is_set(): start_time = time.time() msg = self.lowstate_subscriber.Read() if msg is not None: @@ -218,10 +228,17 @@ class UnitreeG1(Robot): pass def connect(self, calibrate: bool = True) -> None: # connect to DDS - ChannelFactoryInitialize(0) + if self.config.is_simulation: + self.ChannelFactoryInitialize(0, "lo") + self.mujoco_env = make_env("lerobot/unitree-g1-mujoco", trust_remote_code=True) + else: + self.ChannelFactoryInitialize(0) def disconnect(self): - pass + self._shutdown_event.set() + self.subscribe_thread.join(timeout=2.0) + if self.config.is_simulation: + self.mujoco_env["hub_env"][0].envs[0].kill_sim() def get_observation(self) -> dict[str, Any]: return self.lowstate_buffer.get_data()