wangshuxun/Xarm-DataCollection
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: align GELLO from its current passive pose

Browse Source
This commit is contained in:
ChenYuhan 2026-08-10 17:47:48 +08:00
parent 6cc1a3df95
commit 2c59ff82ba
9 changed files with 97 additions and 496 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
config/gello/xarm7_gello_record_config.yaml
View File

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

37
config/gello/xarm7_gello_reset_calibration.yaml
View File

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

339
scripts/reset_gello_xarm7.py
View File

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

1
src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py
View File

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

1
src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py
View File

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

96
src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py
View File

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

12
src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop_config.py
View File

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

98
tests/test_gello_reset.py
View File

@ -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",
]

2
third_party/gello_software vendored

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