diff --git a/.gitignore b/.gitignore index 8aa32ef..c48b1d7 100644 --- a/.gitignore +++ b/.gitignore @@ -89,4 +89,8 @@ models/ *.xvcd ufactory_usage/ .history/ -datasets/ \ No newline at end of file +datasets/ + +# Local checkout used for GELLO hardware development. Runtime fixes live in +# the main package so the repository does not depend on a modified submodule. +third_party/gello_software/ diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 73ec519..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "third_party/gello_software"] - path = third_party/gello_software - url = https://github.com/xArm-Developer/gello_software.git diff --git a/pyproject.toml b/pyproject.toml index 6f97f85..28081ce 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -45,7 +45,7 @@ uf-camera-test = "lerobot_robot_ufactory.scripts.uf_camera_test:main" [project.optional-dependencies] # GELLO 遥操作 gello = [ - "gello", + "gello @ git+https://github.com/xArm-Developer/gello_software.git", "dynamixel-sdk>=4.0.5", ] # SpaceMouse 遥操作 @@ -70,9 +70,6 @@ where = ["src"] [tool.setuptools.package-data] "lerobot_robot_ufactory.devices.umi.xvlib" = ["*.so", "*.so.*"] -[tool.uv.sources] -gello = { path = "third_party/gello_software", editable = true } - [tool.ruff] line-length = 100 target-version = "py310" diff --git a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_adapter.py b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_adapter.py new file mode 100644 index 0000000..dffa7f9 --- /dev/null +++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_adapter.py @@ -0,0 +1,154 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional, Sequence, Tuple + +import numpy as np +from dynamixel_sdk import COMM_SUCCESS +from dynamixel_sdk.robotis_def import ( + DXL_HIBYTE, + DXL_HIWORD, + DXL_LOBYTE, + DXL_LOWORD, +) +from gello.dynamixel import driver as driver_module +from gello.dynamixel.driver import DynamixelDriver +from gello.robots.dynamixel import DynamixelRobot + + +class SafeDynamixelDriver(DynamixelDriver): + """GELLO driver with serialized writes and complete torque cleanup.""" + + def set_joints(self, joint_angles: Sequence[float]) -> None: + if len(joint_angles) != len(self._ids): + raise ValueError("joint_angles must match the configured Dynamixel IDs") + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + if self._is_fake: + self._fake_joint_angles = np.asarray(joint_angles, dtype=float) + return + + with self._lock: + try: + for dxl_id, angle in zip(self._ids, joint_angles, strict=True): + position_value = int(angle * 2048 / np.pi) + parameter = [ + DXL_LOBYTE(DXL_LOWORD(position_value)), + DXL_HIBYTE(DXL_LOWORD(position_value)), + DXL_LOBYTE(DXL_HIWORD(position_value)), + DXL_HIBYTE(DXL_HIWORD(position_value)), + ] + if not self._groupSyncWrite.addParam(dxl_id, parameter): + raise RuntimeError( + f"Failed to set joint angle for Dynamixel ID {dxl_id}" + ) + + result = self._groupSyncWrite.txPacket() + if result != COMM_SUCCESS: + detail = self._packetHandler.getTxRxResult(result) + raise RuntimeError( + f"Failed to syncwrite goal position: {detail} ({result})" + ) + finally: + self._groupSyncWrite.clearParam() + + def set_torque_mode(self, enable: bool) -> None: + if self._is_fake: + self._torque_enabled = enable + return + + torque_value = driver_module.TORQUE_ENABLE if enable else driver_module.TORQUE_DISABLE + failures = [] + with self._lock: + for dxl_id in self._ids: + result, error = self._packetHandler.write1ByteTxRx( + self._portHandler, + dxl_id, + driver_module.ADDR_TORQUE_ENABLE, + torque_value, + ) + if result != COMM_SUCCESS: + detail = self._packetHandler.getTxRxResult(result) + failures.append(f"ID {dxl_id}: {detail} ({result})") + continue + if error == 0: + continue + + if not enable: + state, read_result, _ = self._packetHandler.read1ByteTxRx( + self._portHandler, + dxl_id, + driver_module.ADDR_TORQUE_ENABLE, + ) + if read_result == COMM_SUCCESS and state == driver_module.TORQUE_DISABLE: + continue + + detail = self._packetHandler.getRxPacketError(error) + failures.append(f"ID {dxl_id}: {detail} ({error})") + + if failures: + raise RuntimeError("Failed to set torque mode: " + "; ".join(failures)) + self._torque_enabled = enable + + +class ContinuousDynamixelRobot(DynamixelRobot): + """Dynamixel GELLO whose arm joints remain continuous across encoder wrap.""" + + def get_joint_state(self) -> np.ndarray: + pos = (self._driver.get_joints() - self._joint_offsets) * self._joint_signs + if len(pos) != self.num_dofs(): + raise RuntimeError("Unexpected Dynamixel joint count") + + arm_dofs = len(pos) - 1 if self.gripper_open_close is not None else len(pos) + if self._last_pos is not None: + pos[:arm_dofs] += 2 * np.pi * np.round( + (self._last_pos[:arm_dofs] - pos[:arm_dofs]) / (2 * np.pi) + ) + + if self.gripper_open_close is not None: + gripper_open, gripper_close = self.gripper_open_close + gripper_pos = (pos[-1] - gripper_open) / (gripper_close - gripper_open) + pos[-1] = min(max(0.0, gripper_pos), 1.0) + + if self._last_pos is None: + self._last_pos = pos + else: + pos = self._last_pos * (1 - self._alpha) + pos * self._alpha + self._last_pos = pos + return pos + + +@dataclass +class PatchedDynamixelRobotConfig: + joint_ids: Sequence[int] + joint_offsets: Sequence[float] + joint_signs: Sequence[int] + gripper_config: Optional[Tuple[int, float, float]] + + def __post_init__(self) -> None: + if len(self.joint_ids) != len(self.joint_offsets): + raise ValueError("joint_ids and joint_offsets must have the same length") + if len(self.joint_ids) != len(self.joint_signs): + raise ValueError("joint_ids and joint_signs must have the same length") + + def make_robot( + self, + port: str = "/dev/ttyUSB0", + start_joints: Optional[np.ndarray] = None, + ) -> ContinuousDynamixelRobot: + # Upstream DynamixelRobot imports its driver inside __init__. Replace + # that symbol only while constructing this instance. + original_driver = driver_module.DynamixelDriver + driver_module.DynamixelDriver = SafeDynamixelDriver + try: + return ContinuousDynamixelRobot( + joint_ids=self.joint_ids, + joint_offsets=self.joint_offsets, + joint_signs=self.joint_signs, + real=True, + port=port, + gripper_config=self.gripper_config, + start_joints=start_joints, + ) + finally: + driver_module.DynamixelDriver = original_driver diff --git a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py index 629e6de..19bf514 100644 --- a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py +++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py @@ -4,6 +4,7 @@ import time import numpy as np from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from ..base_teleop import UFBaseTeleop +from .gello_adapter import PatchedDynamixelRobotConfig from .gello_teleop_config import GelloTeleopConfig @@ -25,8 +26,6 @@ class GelloTeleop(UFBaseTeleop): self._needs_alignment = True self._is_calibrated = True # CHECK!! - from gello.agents.gello_agent import DynamixelRobotConfig - joint_offsets = [0.0] * len(self.config.joint_ids) self._align_gripper_to_current = self.config.gripper_open_deg is None if self.config.gripper_id >= 0: @@ -52,7 +51,7 @@ class GelloTeleop(UFBaseTeleop): "joint_offsets": joint_offsets, "gripper_config": gripper_config } - self._dynamixel_robo_config = DynamixelRobotConfig(**param_dict) + self._dynamixel_robo_config = PatchedDynamixelRobotConfig(**param_dict) print(self._dynamixel_robo_config) self.dof = len(self.config.joint_ids) diff --git a/tests/test_gello_reset.py b/tests/test_gello_reset.py index daf7b67..4509715 100644 --- a/tests/test_gello_reset.py +++ b/tests/test_gello_reset.py @@ -3,7 +3,9 @@ import pytest from lerobot_robot_ufactory.teleoperators.gello_teleop import gello_teleop as gello_module from lerobot_robot_ufactory.scripts.uf_lerobot_record import _prepare_recording_episode -from gello.robots.dynamixel import DynamixelRobot +from lerobot_robot_ufactory.teleoperators.gello_teleop.gello_adapter import ( + ContinuousDynamixelRobot, +) class FakeDriver: @@ -88,7 +90,7 @@ def test_fixed_gripper_endpoints_are_not_shifted_during_arm_alignment(): def test_dynamixel_arm_joint_is_continuous_across_encoder_wrap(): - robot = DynamixelRobot( + robot = ContinuousDynamixelRobot( joint_ids=[1], joint_offsets=[0.0], joint_signs=[1], diff --git a/third_party/gello_software b/third_party/gello_software deleted file mode 160000 index 313cdd8..0000000 --- a/third_party/gello_software +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 313cdd831f4e87bcbb056dbd1f68d1e8898a6681 diff --git a/uv.lock b/uv.lock index 9874c76..f1bb88e 100644 --- a/uv.lock +++ b/uv.lock @@ -935,16 +935,13 @@ http = [ [[package]] name = "gello" version = "0.0.1" -source = { editable = "third_party/gello_software" } +source = { git = "https://github.com/xArm-Developer/gello_software.git#523437fa4615155813124efeae26fdef32e546a0" } dependencies = [ { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version == '3.11.*'" }, { name = "numpy", version = "2.5.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.12'" }, ] -[package.metadata] -requires-dist = [{ name = "numpy" }] - [[package]] name = "gitdb" version = "4.0.12" @@ -1254,7 +1251,7 @@ spacemouse = [ [package.metadata] requires-dist = [ { name = "dynamixel-sdk", marker = "extra == 'gello'", specifier = ">=4.0.5" }, - { name = "gello", marker = "extra == 'gello'", editable = "third_party/gello_software" }, + { name = "gello", marker = "extra == 'gello'", git = "https://github.com/xArm-Developer/gello_software.git" }, { name = "lerobot", extras = ["intelrealsense"], specifier = "==0.4.3" }, { name = "mypy", marker = "extra == 'dev'", specifier = ">=1.10" }, { name = "numpy", specifier = ">=1.24" },