diff --git a/config/gello/xarm7_gello_record_config.yaml b/config/gello/xarm7_gello_record_config.yaml index 9b35811..c9d9fc4 100644 --- a/config/gello/xarm7_gello_record_config.yaml +++ b/config/gello/xarm7_gello_record_config.yaml @@ -15,14 +15,9 @@ teleop: port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0" joint_ids: [1, 2, 3, 4, 5, 6, 7] joint_signs: [1, 1, 1, 1, 1, 1, 1] - # Fixed from xarm7_gello_reset_calibration.yaml. The captured GELLO pose - # below maps to the xArm SDK initial point instead of recalibrating at startup. - joint_offsets: [89.472656, 239.794922, 176.396484, 155.039062, 181.669922, 126.621094, 176.132812] - start_joints: [0, -30, 0, 0, 0, 30, 0] gripper_id: 8 - gripper_open_deg: 198.017578 + gripper_open_deg: 198.28125 gripper_close_deg: 155.75 - reset_speed_deg_s: 10.0 dataset: # root of local repo: /home//.cache/huggingface/lerobot (default) diff --git a/config/gello/xarm7_gello_reset_calibration.yaml b/config/gello/xarm7_gello_reset_calibration.yaml deleted file mode 100644 index 0232d70..0000000 --- a/config/gello/xarm7_gello_reset_calibration.yaml +++ /dev/null @@ -1,37 +0,0 @@ -schema_version: 1 -description: GELLO pose matching the xArm7 SDK initial point -robot_initial_joints_deg: -- 0.0 -- -30.0 -- 0.0 -- 0.0 -- 0.0 -- 30.0 -- 0.0 -dynamixel_ids: -- 1 -- 2 -- 3 -- 4 -- 5 -- 6 -- 7 -- 8 -target_raw_counts: -- 1018 -- 2387 -- 2007 -- 1764 -- 2067 -- 1782 -- 2004 -- 2253 -target_encoder_deg: -- 89.472656 -- 209.794922 -- 176.396484 -- 155.039062 -- 181.669922 -- 156.621094 -- 176.132812 -- 198.017578 diff --git a/scripts/reset_gello_xarm7.py b/scripts/reset_gello_xarm7.py deleted file mode 100644 index a082d85..0000000 --- a/scripts/reset_gello_xarm7.py +++ /dev/null @@ -1,339 +0,0 @@ -#!/usr/bin/env python3 -"""Calibrate and reset a GELLO leader to the xArm7 SDK initial pose.""" - -from __future__ import annotations - -import argparse -import time -from pathlib import Path -from typing import Sequence - -import numpy as np -import yaml -from dynamixel_sdk import COMM_SUCCESS, GroupSyncWrite, PacketHandler, PortHandler - - -DEFAULT_PORT = ( - "/dev/serial/by-id/" - "usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0" -) -DEFAULT_CALIBRATION = Path("config/gello/xarm7_gello_reset_calibration.yaml") -ROBOT_INITIAL_JOINTS_DEG = [0.0, -30.0, 0.0, 0.0, 0.0, 30.0, 0.0] -JOINT_IDS = list(range(1, 8)) -GRIPPER_ID = 8 -BAUDRATE = 57600 - -ADDR_OPERATING_MODE = 11 -ADDR_TORQUE_ENABLE = 64 -ADDR_HARDWARE_ERROR = 70 -ADDR_GOAL_POSITION = 116 -ADDR_PRESENT_CURRENT = 126 -ADDR_PRESENT_POSITION = 132 -POSITION_CONTROL_MODE = 3 - - -class GelloBus: - def __init__(self, port: str, ids: Sequence[int]) -> None: - self.ids = list(ids) - self.port = PortHandler(port) - self.packet = PacketHandler(2.0) - self.writer = GroupSyncWrite( - self.port, self.packet, ADDR_GOAL_POSITION, 4 - ) - - if not self.port.openPort(): - raise RuntimeError(f"Failed to open GELLO port: {port}") - if not self.port.setBaudRate(BAUDRATE): - self.port.closePort() - raise RuntimeError(f"Failed to set GELLO baud rate to {BAUDRATE}") - - def close(self) -> None: - self.writer.clearParam() - self.port.closePort() - - def _check(self, operation: str, dxl_id: int, comm: int, error: int) -> None: - if comm != COMM_SUCCESS: - detail = self.packet.getTxRxResult(comm) - raise RuntimeError( - f"{operation} failed for Dynamixel {dxl_id}: {detail} ({comm})" - ) - if error != 0: - detail = self.packet.getRxPacketError(error) - raise RuntimeError( - f"{operation} failed for Dynamixel {dxl_id}: {detail} ({error})" - ) - - def read_u8(self, dxl_id: int, address: int) -> int: - value, comm, error = self.packet.read1ByteTxRx( - self.port, dxl_id, address - ) - self._check("read", dxl_id, comm, error) - return value - - def read_i16(self, dxl_id: int, address: int) -> int: - value, comm, error = self.packet.read2ByteTxRx( - self.port, dxl_id, address - ) - self._check("read", dxl_id, comm, error) - return value - 0x10000 if value >= 0x8000 else value - - def read_i32(self, dxl_id: int, address: int) -> int: - value, comm, error = self.packet.read4ByteTxRx( - self.port, dxl_id, address - ) - self._check("read", dxl_id, comm, error) - return value - 0x100000000 if value >= 0x80000000 else value - - def positions(self) -> np.ndarray: - return np.asarray( - [self.read_i32(dxl_id, ADDR_PRESENT_POSITION) for dxl_id in self.ids], - dtype=float, - ) - - def set_torque(self, enabled: bool) -> None: - value = 1 if enabled else 0 - for dxl_id in self.ids: - comm, error = self.packet.write1ByteTxRx( - self.port, dxl_id, ADDR_TORQUE_ENABLE, value - ) - self._check("set torque", dxl_id, comm, error) - - def verify_position_mode(self) -> None: - for dxl_id in self.ids: - mode = self.read_u8(dxl_id, ADDR_OPERATING_MODE) - if mode != POSITION_CONTROL_MODE: - raise RuntimeError( - f"Dynamixel {dxl_id} is in mode {mode}, expected position mode 3" - ) - - def write_positions(self, raw_positions: Sequence[int]) -> None: - self.writer.clearParam() - try: - for dxl_id, raw_position in zip(self.ids, raw_positions, strict=True): - encoded = int(raw_position) & 0xFFFFFFFF - data = list(encoded.to_bytes(4, byteorder="little", signed=False)) - if not self.writer.addParam(dxl_id, data): - raise RuntimeError( - f"Failed to add goal position for Dynamixel {dxl_id}" - ) - comm = self.writer.txPacket() - if comm != COMM_SUCCESS: - detail = self.packet.getTxRxResult(comm) - raise RuntimeError( - f"SyncWrite failed: {detail} ({comm})" - ) - finally: - self.writer.clearParam() - - -def counts_to_degrees(values: Sequence[float]) -> np.ndarray: - return np.asarray(values, dtype=float) / 4096.0 * 360.0 - - -def save_calibration(path: Path, ids: Sequence[int], targets: Sequence[int]) -> None: - data = { - "schema_version": 1, - "description": "GELLO pose matching the xArm7 SDK initial point", - "robot_initial_joints_deg": ROBOT_INITIAL_JOINTS_DEG, - "dynamixel_ids": list(ids), - "target_raw_counts": [int(value) for value in targets], - "target_encoder_deg": [ - round(float(value), 6) for value in counts_to_degrees(targets) - ], - } - path.parent.mkdir(parents=True, exist_ok=True) - path.write_text(yaml.safe_dump(data, sort_keys=False), encoding="utf-8") - - -def load_calibration(path: Path, expected_ids: Sequence[int]) -> np.ndarray: - if not path.is_file(): - raise FileNotFoundError( - f"Calibration file not found: {path}. Run this script with --calibrate first." - ) - data = yaml.safe_load(path.read_text(encoding="utf-8")) - ids = data.get("dynamixel_ids") - targets = data.get("target_raw_counts") - if ids != list(expected_ids): - raise ValueError(f"Calibration IDs {ids} do not match expected IDs {expected_ids}") - if not isinstance(targets, list) or len(targets) != len(expected_ids): - raise ValueError("Calibration target_raw_counts has an invalid length") - return np.asarray(targets, dtype=float) - - -def calibrate(args: argparse.Namespace) -> None: - ids = JOINT_IDS + ([GRIPPER_ID] if not args.no_gripper else []) - bus = GelloBus(args.port, ids) - try: - bus.set_torque(False) - print("GELLO torque is disabled.") - if not args.yes: - print( - "Manually place GELLO in the physical pose matching xArm7 " - f"{ROBOT_INITIAL_JOINTS_DEG} degrees." - ) - print("Keep the gripper open, then press Enter to capture this pose.") - input() - targets = np.rint(bus.positions()).astype(int) - save_calibration(args.calibration, ids, targets) - print(f"Saved calibration: {args.calibration}") - print("Target encoder degrees:", counts_to_degrees(targets).round(2).tolist()) - finally: - try: - bus.set_torque(False) - finally: - bus.close() - - -def reset(args: argparse.Namespace) -> None: - ids = JOINT_IDS + ([GRIPPER_ID] if not args.no_gripper else []) - targets = load_calibration(args.calibration, ids) - bus = GelloBus(args.port, ids) - torque_enabled = False - completed = False - try: - bus.set_torque(False) - bus.verify_position_mode() - current = bus.positions() - move_deg = counts_to_degrees(targets - current) - max_move_deg = float(np.max(np.abs(move_deg))) - print("Current encoder degrees:", counts_to_degrees(current).round(2).tolist()) - print("Target encoder degrees: ", counts_to_degrees(targets).round(2).tolist()) - print("Required move degrees: ", move_deg.round(2).tolist()) - print(f"Maximum move: {max_move_deg:.2f} degrees") - - if max_move_deg > args.max_move_deg: - raise RuntimeError( - f"Refusing reset: {max_move_deg:.2f} degree move exceeds " - f"--max-move-deg={args.max_move_deg:.2f}" - ) - if not args.yes: - answer = input("Type 'yes' to enable GELLO torque and reset: ").strip() - if answer.lower() != "yes": - print("Reset cancelled.") - return - - bus.write_positions(np.rint(current).astype(int)) - for remaining in (3, 2, 1): - print(f"Enabling torque in {remaining}...") - time.sleep(1.0) - bus.set_torque(True) - torque_enabled = True - - duration = max(0.5, max_move_deg / args.speed_deg_s) - steps = max(1, int(duration * args.control_hz)) - started = time.monotonic() - for step in range(1, steps + 1): - u = step / steps - smooth = u * u * (3.0 - 2.0 * u) - command = np.rint(current + (targets - current) * smooth).astype(int) - bus.write_positions(command) - - if step % max(1, int(args.control_hz / 5)) == 0: - currents = [ - bus.read_i16(dxl_id, ADDR_PRESENT_CURRENT) for dxl_id in ids - ] - errors = [ - bus.read_u8(dxl_id, ADDR_HARDWARE_ERROR) for dxl_id in ids - ] - if any(errors): - raise RuntimeError(f"Dynamixel hardware errors: {errors}") - if max(abs(value) for value in currents) > args.max_current_raw: - raise RuntimeError( - f"Current safety threshold exceeded: {currents}" - ) - - delay = started + step / args.control_hz - time.monotonic() - if delay > 0: - time.sleep(delay) - - deadline = time.monotonic() + args.settle_timeout_s - final = bus.positions() - while time.monotonic() < deadline: - error_deg = counts_to_degrees(final - targets) - if float(np.max(np.abs(error_deg))) <= args.tolerance_deg: - break - bus.write_positions(np.rint(targets).astype(int)) - time.sleep(1.0 / args.control_hz) - final = bus.positions() - else: - error_deg = counts_to_degrees(final - targets) - raise RuntimeError( - "GELLO did not reach the calibrated pose; final errors: " - f"{error_deg.round(2).tolist()} degrees" - ) - - print("Final encoder degrees:", counts_to_degrees(final).round(2).tolist()) - print("GELLO reset completed.") - completed = True - finally: - if torque_enabled and (not completed or args.release_after_reset): - try: - bus.set_torque(False) - print("GELLO torque disabled.") - except Exception as exc: - print(f"WARNING: failed to disable GELLO torque: {exc}") - bus.close() - - if completed and not args.release_after_reset: - print("GELLO torque remains enabled and is holding the calibrated pose.") - - -def release(args: argparse.Namespace) -> None: - ids = JOINT_IDS + ([GRIPPER_ID] if not args.no_gripper else []) - bus = GelloBus(args.port, ids) - try: - bus.set_torque(False) - print("GELLO torque disabled.") - finally: - bus.close() - - -def parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser( - description=( - "Calibrate or reset GELLO to the physical pose matching the xArm7 " - "SDK initial joints [0, -30, 0, 0, 0, 30, 0] degrees." - ) - ) - parser.add_argument("--port", default=DEFAULT_PORT) - parser.add_argument("--calibration", type=Path, default=DEFAULT_CALIBRATION) - mode = parser.add_mutually_exclusive_group() - mode.add_argument("--calibrate", action="store_true") - mode.add_argument( - "--release", - action="store_true", - help="Disable GELLO torque without moving it", - ) - parser.add_argument("--no-gripper", action="store_true") - parser.add_argument("--yes", action="store_true", help="Skip interactive confirmation") - parser.add_argument("--speed-deg-s", type=float, default=20.0) - parser.add_argument("--control-hz", type=float, default=50.0) - parser.add_argument("--max-move-deg", type=float, default=90.0) - parser.add_argument("--max-current-raw", type=int, default=300) - parser.add_argument("--tolerance-deg", type=float, default=3.0) - parser.add_argument("--settle-timeout-s", type=float, default=5.0) - parser.add_argument( - "--release-after-reset", - action="store_true", - help="Disable torque after a successful reset instead of holding the pose", - ) - args = parser.parse_args() - if args.speed_deg_s <= 0 or args.control_hz <= 0: - parser.error("--speed-deg-s and --control-hz must be positive") - if args.max_move_deg <= 0 or args.tolerance_deg <= 0: - parser.error("--max-move-deg and --tolerance-deg must be positive") - return args - - -def main() -> None: - args = parse_args() - if args.calibrate: - calibrate(args) - elif args.release: - release(args) - else: - reset(args) - - -if __name__ == "__main__": - main() diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py index 3947fab..2a86b77 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py +++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py @@ -478,7 +478,6 @@ def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode): if is_uf_teleop: obs = robot.get_observation() - teleop.reset_to_robot_observation(obs) teleop.set_teleop_enabled(True, obs) diff --git a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py index bbe7ad7..9bc7a9d 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py +++ b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py @@ -101,7 +101,6 @@ def teleop_loop(cfg: TeleopConfig): reset() if is_uf_teleop: obs = robot.get_observation() - teleop.reset_to_robot_observation(obs) teleop.set_teleop_enabled(True, obs) is_reset = is_uf_teleop 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 0bfc2b5..629e6de 100644 --- a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py +++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py @@ -1,7 +1,6 @@ #!/usr/bin/env python import logging import time -import math import numpy as np from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from ..base_teleop import UFBaseTeleop @@ -10,10 +9,6 @@ from .gello_teleop_config import GelloTeleopConfig logger = logging.getLogger(__name__) -GELLO_RESET_TOLERANCE_DEG = 2.0 -GELLO_RESET_CONTROL_HZ = 50.0 -GELLO_RESET_TIMEOUT_MARGIN_S = 5.0 - class GelloTeleop(UFBaseTeleop): """ GELLO for xArm tele-op, ref: https://wuphilipp.github.io/gello_site/ @@ -30,34 +25,19 @@ class GelloTeleop(UFBaseTeleop): self._needs_alignment = True self._is_calibrated = True # CHECK!! - from gello.dynamixel.driver import DynamixelDriver from gello.agents.gello_agent import DynamixelRobotConfig - # auto get joint offset from gello - joint_ids = [] - joint_ids.extend(self.config.joint_ids) - if self.config.gripper_id >= 0: - joint_ids.append(self.config.gripper_id) - driver = DynamixelDriver(joint_ids, port=self.config.port, baudrate=57600) - for _ in range(10): - driver.get_joints() # warmup - curr_joints = driver.get_joints() - driver.close() - start_joints = list(map(math.radians, self.config.start_joints)) - if self.config.joint_offsets is not None: - joint_offsets = list(map(math.radians, self.config.joint_offsets)) - else: - joint_offsets = [] - for i in range(len(start_joints)): - offset = curr_joints[i] - start_joints[i] / self.config.joint_signs[i] - joint_offsets.append(offset) + 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: if self.config.gripper_open_deg is not None: gripper_open_deg = self.config.gripper_open_deg gripper_close_deg = self.config.gripper_close_deg else: - gripper_open_deg = np.rad2deg(curr_joints[-1]) - 0.2 - gripper_close_deg = np.rad2deg(curr_joints[-1]) - 42 + # Only the range matters. It is shifted to the current GELLO + # gripper position whenever teleoperation is enabled. + gripper_open_deg = 0.0 + gripper_close_deg = -42.0 gripper_config = [ self.config.gripper_id, gripper_open_deg, @@ -74,7 +54,7 @@ class GelloTeleop(UFBaseTeleop): } self._dynamixel_robo_config = DynamixelRobotConfig(**param_dict) print(self._dynamixel_robo_config) - self.dof = len(start_joints) + self.dof = len(self.config.joint_ids) @property def action_features(self) -> dict: @@ -138,67 +118,49 @@ class GelloTeleop(UFBaseTeleop): pass def reset_to_robot_observation(self, obs): - """Move the physical Gello to the robot's post-reset joint state.""" + """Map the current passive GELLO pose to the robot's current pose.""" if not self._is_connected: raise DeviceNotConnectedError("Gello teleop is not connected") self._teleop_enabled = False gello_robot = self.gello_agent._robot driver = gello_robot._driver + gello_robot.set_torque_mode(False) current_raw = np.asarray(driver.get_joints(), dtype=float) - target_raw = current_raw.copy() signs = np.asarray(gello_robot._joint_signs, dtype=float) - offsets = np.asarray(gello_robot._joint_offsets, dtype=float) - target_robot_joints = np.asarray( + robot_joints = np.asarray( [obs[f"J{i + 1}.pos"] for i in range(self.dof)], dtype=float ) - target_raw[: self.dof] = target_robot_joints * signs[: self.dof] + offsets[: self.dof] + gello_robot._joint_offsets[: self.dof] = ( + current_raw[: self.dof] - robot_joints * signs[: self.dof] + ) - if gello_robot.gripper_open_close is not None and len(target_raw) > self.dof: + if ( + self._align_gripper_to_current + and gello_robot.gripper_open_close is not None + and len(current_raw) > self.dof + ): gripper_pos = float(obs.get("gripper.pos", 0.0)) gripper_open, gripper_close = gello_robot.gripper_open_close gripper_pos = min(max(gripper_pos, 0.0), 1.0) - target_raw[self.dof] = gripper_open + gripper_pos * (gripper_close - gripper_open) - - arm_delta = np.max(np.abs(target_raw[: self.dof] - current_raw[: self.dof])) - reset_speed_rad_s = math.radians(self.config.reset_speed_deg_s) - duration_s = max(0.5, float(arm_delta / reset_speed_rad_s)) - deadline = time.perf_counter() + duration_s + GELLO_RESET_TIMEOUT_MARGIN_S - success = False - - try: - gello_robot.set_torque_mode(True) - start_t = time.perf_counter() - while True: - elapsed_s = time.perf_counter() - start_t - progress = min(elapsed_s / duration_s, 1.0) - command = current_raw + (target_raw - current_raw) * progress - driver.set_joints(command.tolist()) - if progress >= 1.0: - break - time.sleep(1.0 / GELLO_RESET_CONTROL_HZ) - - while time.perf_counter() < deadline: - measured_raw = np.asarray(driver.get_joints(), dtype=float) - if np.max(np.abs(measured_raw - target_raw)) <= math.radians(GELLO_RESET_TOLERANCE_DEG): - success = True - break - driver.set_joints(target_raw.tolist()) - time.sleep(1.0 / GELLO_RESET_CONTROL_HZ) - - if not success: - raise RuntimeError("Gello did not reach the robot initial point before timeout") - finally: - gello_robot.set_torque_mode(False) - gello_robot._last_pos = None + gripper_span = gripper_close - gripper_open + gripper_open = current_raw[self.dof] - gripper_pos * gripper_span + gello_robot.gripper_open_close = ( + gripper_open, + gripper_open + gripper_span, + ) + gello_robot._last_pos = None self._needs_alignment = False + logger.info("Current GELLO pose aligned to current robot observation") def set_teleop_enabled(self, enabled: bool, obs=None): if enabled and not self._is_connected: raise DeviceNotConnectedError("Gello teleop is not connected") - if enabled and self._needs_alignment and obs is not None: + if enabled and self._needs_alignment: + if obs is None: + raise ValueError("Robot observation is required to enable GELLO teleoperation") self.reset_to_robot_observation(obs) if not enabled and self._is_connected and hasattr(self, "gello_agent"): self.gello_agent._robot.set_torque_mode(False) diff --git a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py index 9e4173f..774f819 100644 --- a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py +++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py @@ -14,16 +14,16 @@ class GelloTeleopConfig(TeleoperatorConfig): # Others: Calibration angles, joint directions etc joint_ids: Tuple[int, ...] = (1, 2, 3, 4, 5, 6, 7) joint_signs: Tuple[int, ...] = (1, 1, 1, 1, 1, 1, 1) # if follow the original open-sourced gello xarm7 setup - # Raw Dynamixel zero offsets in degrees. When omitted, the current GELLO - # pose is treated as start_joints for backwards compatibility. + # Accepted for compatibility but ignored: arm zero offsets are captured + # from the current GELLO and xArm poses whenever teleoperation is enabled. joint_offsets: Optional[Tuple[float, ...]] = None - # GELLO encoder calibration reference; this is not the xArm reset target. + # Retained for compatibility with existing xArm5/xArm6 YAML files. GELLO + # alignment now always uses its current pose when teleoperation is enabled. start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # ° gripper_id: int = 8 # -1: no gripper gripper_open_deg: Optional[float] = None gripper_close_deg: Optional[float] = None - reset_speed_deg_s: float = 10.0 - torque_joint_ids: Tuple[int, ...] = None # deprecated; reset controls all GELLO joints. + torque_joint_ids: Tuple[int, ...] = None # deprecated def __post_init__(self): self.id = 'gello_teleop' if self.id is None else self.id @@ -35,5 +35,3 @@ class GelloTeleopConfig(TeleoperatorConfig): raise ValueError("joint_ids and joint_offsets must have the same length") if (self.gripper_open_deg is None) != (self.gripper_close_deg is None): raise ValueError("gripper_open_deg and gripper_close_deg must be set together") - if self.reset_speed_deg_s <= 0: - raise ValueError("reset_speed_deg_s must be positive") diff --git a/tests/test_gello_reset.py b/tests/test_gello_reset.py index 5d4e59d..daf7b67 100644 --- a/tests/test_gello_reset.py +++ b/tests/test_gello_reset.py @@ -3,12 +3,12 @@ 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 class FakeDriver: - def __init__(self, positions, follow_commands=True): + def __init__(self, positions): self.positions = np.asarray(positions, dtype=float) - self.follow_commands = follow_commands self.commands = [] def get_joints(self): @@ -16,16 +16,14 @@ class FakeDriver: def set_joints(self, positions): self.commands.append(np.asarray(positions, dtype=float)) - if self.follow_commands: - self.positions = self.commands[-1].copy() def close(self): pass class FakeGelloRobot: - def __init__(self, follow_commands=True): - self._driver = FakeDriver([0.0, 0.0, 0.0], follow_commands=follow_commands) + def __init__(self): + self._driver = FakeDriver([0.7, -0.2, 1.5]) self._joint_signs = np.array([1.0, -1.0, 1.0]) self._joint_offsets = np.array([0.1, 0.2, 0.0]) self.gripper_open_close = (0.0, 1.0) @@ -36,29 +34,19 @@ class FakeGelloRobot: self.torque_calls.append(enabled) -def make_teleop(robot): +def make_teleop(robot, align_gripper_to_current=True): teleop = gello_module.GelloTeleop.__new__(gello_module.GelloTeleop) teleop.id = "test_gello" teleop._is_connected = True teleop._teleop_enabled = False teleop._needs_alignment = True + teleop._align_gripper_to_current = align_gripper_to_current teleop.dof = 2 teleop.gello_agent = type("FakeAgent", (), {"_robot": robot})() return teleop -def patch_clock(monkeypatch): - clock = [0.0] - monkeypatch.setattr(gello_module.time, "perf_counter", lambda: clock[0]) - monkeypatch.setattr( - gello_module.time, - "sleep", - lambda seconds: clock.__setitem__(0, clock[0] + seconds), - ) - - -def test_gello_reset_moves_to_robot_observation_and_disables_torque(monkeypatch): - patch_clock(monkeypatch) +def test_gello_alignment_maps_current_pose_without_moving(): robot = FakeGelloRobot() teleop = make_teleop(robot) @@ -66,29 +54,58 @@ def test_gello_reset_moves_to_robot_observation_and_disables_torque(monkeypatch) {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5} ) - assert robot.torque_calls == [True, False] - assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5]) + assert robot.torque_calls == [False] + assert robot._driver.commands == [] + assert np.allclose(robot._joint_offsets[:2], [0.4, -0.6]) + assert np.allclose(robot.gripper_open_close, [1.0, 2.0]) assert robot._last_pos is None assert teleop._teleop_enabled is False assert teleop._needs_alignment is False -def test_gello_reset_failure_leaves_torque_off_and_teleop_disabled(monkeypatch): - patch_clock(monkeypatch) - robot = FakeGelloRobot(follow_commands=False) +def test_gello_enable_requires_robot_observation(): + robot = FakeGelloRobot() teleop = make_teleop(robot) - with pytest.raises(RuntimeError, match="did not reach"): - teleop.reset_to_robot_observation( - {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5} - ) + with pytest.raises(ValueError, match="Robot observation"): + teleop.set_teleop_enabled(True) - assert robot.torque_calls == [True, False] + assert robot.torque_calls == [] assert teleop._teleop_enabled is False -def test_gello_enable_after_pause_realigns_before_output(monkeypatch): - patch_clock(monkeypatch) +def test_fixed_gripper_endpoints_are_not_shifted_during_arm_alignment(): + robot = FakeGelloRobot() + robot.gripper_open_close = (3.45, 2.72) + teleop = make_teleop(robot, align_gripper_to_current=False) + + teleop.reset_to_robot_observation( + {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5} + ) + + assert robot.gripper_open_close == (3.45, 2.72) + assert robot._driver.commands == [] + + +def test_dynamixel_arm_joint_is_continuous_across_encoder_wrap(): + robot = DynamixelRobot( + joint_ids=[1], + joint_offsets=[0.0], + joint_signs=[1], + real=False, + ) + robot._alpha = 1.0 + robot._driver._joint_angles = np.array([2 * np.pi - 0.05]) + before_wrap = robot.get_joint_state()[0] + + robot._driver._joint_angles = np.array([0.05]) + after_wrap = robot.get_joint_state()[0] + + assert after_wrap > before_wrap + assert after_wrap - before_wrap == pytest.approx(0.1) + + +def test_gello_enable_after_pause_realigns_current_pose_before_output(): robot = FakeGelloRobot() teleop = make_teleop(robot) @@ -97,10 +114,21 @@ def test_gello_enable_after_pause_realigns_before_output(monkeypatch): {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}, ) - assert robot.torque_calls == [True, False] - assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5]) + assert robot.torque_calls == [False] + assert robot._driver.commands == [] assert teleop._teleop_enabled is True + teleop.set_teleop_enabled(False) + robot._driver.positions = np.array([1.0, 0.5, 1.8]) + teleop.set_teleop_enabled( + True, + {"J1.pos": 0.1, "J2.pos": 0.2, "gripper.pos": 0.25}, + ) + + assert np.allclose(robot._joint_offsets[:2], [0.9, 0.7]) + assert np.allclose(robot.gripper_open_close, [1.55, 2.55]) + assert robot._driver.commands == [] + def test_gello_disconnect_closes_driver(): robot = FakeGelloRobot() @@ -129,15 +157,11 @@ def test_recording_reset_disables_before_robot_and_enables_after_alignment(): def set_teleop_enabled(self, enabled, obs=None): calls.append(f"teleop_{enabled}") - def reset_to_robot_observation(self, obs): - calls.append("gello_alignment") - _prepare_recording_episode(FakeRobot(), FakeTeleop(), True, False) assert calls == [ "teleop_False", "robot_reset", "observation", - "gello_alignment", "teleop_True", ] diff --git a/third_party/gello_software b/third_party/gello_software index b543065..313cdd8 160000 --- a/third_party/gello_software +++ b/third_party/gello_software @@ -1 +1 @@ -Subproject commit b543065ca36f7a444f7f5d110e49b8146d7e1cf3 +Subproject commit 313cdd831f4e87bcbb056dbd1f68d1e8898a6681