feat: align GELLO from its current passive pose

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/<user_name>/.cache/huggingface/lerobot (default)

config/gello/xarm7_gello_reset_calibration.yaml

@@ -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

scripts/reset_gello_xarm7.py

@@ -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()

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)
 
 

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

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_offsets = [0.0] * len(self.config.joint_ids)
-        joint_ids = []
+        self._align_gripper_to_current = self.config.gripper_open_deg is None
-        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)
         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
+                # Only the range matters. It is shifted to the current GELLO
-                gripper_close_deg = np.rad2deg(curr_joints[-1]) - 42
+                # 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)
+            gripper_span = gripper_close - gripper_open
-
+            gripper_open = current_raw[self.dof] - gripper_pos * gripper_span
-        arm_delta = np.max(np.abs(target_raw[: self.dof] - current_raw[: self.dof]))
+            gello_robot.gripper_open_close = (
-        reset_speed_rad_s = math.radians(self.config.reset_speed_deg_s)
+                gripper_open,
-        duration_s = max(0.5, float(arm_delta / reset_speed_rad_s))
+                gripper_open + gripper_span,
-        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
 
+        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)

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
+    # Accepted for compatibility but ignored: arm zero offsets are captured
-    # pose is treated as start_joints for backwards compatibility.
+    # 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
-    torque_joint_ids: Tuple[int, ...] = None  # deprecated; reset controls all GELLO joints.
 
     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")

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):
+    def __init__(self):
-        self._driver = FakeDriver([0.0, 0.0, 0.0], follow_commands=follow_commands)
+        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):
+def test_gello_alignment_maps_current_pose_without_moving():
-    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)
     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 robot.torque_calls == [False]
-    assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5])
+    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):
+def test_gello_enable_requires_robot_observation():
-    patch_clock(monkeypatch)
+    robot = FakeGelloRobot()
-    robot = FakeGelloRobot(follow_commands=False)
     teleop = make_teleop(robot)
 
-    with pytest.raises(RuntimeError, match="did not reach"):
+    with pytest.raises(ValueError, match="Robot observation"):
-        teleop.reset_to_robot_observation(
+        teleop.set_teleop_enabled(True)
-            {"J1.pos": 0.3, "J2.pos": -0.4, "gripper.pos": 0.5}
-        )
 
-    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):
+def test_fixed_gripper_endpoints_are_not_shifted_during_arm_alignment():
-    patch_clock(monkeypatch)
+    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 robot.torque_calls == [False]
-    assert np.allclose(robot._driver.positions, [0.4, 0.6, 0.5])
+    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",
     ]

third_party/gello_software

@@ -1 +1 @@
-Subproject commit b543065ca36f7a444f7f5d110e49b8146d7e1cf3
+Subproject commit 313cdd831f4e87bcbb056dbd1f68d1e8898a6681