feat: add calibrated GELLO reset workflow
This commit is contained in:
parent
d40893cb44
commit
6cc1a3df95
3
.gitmodules
vendored
Normal file
3
.gitmodules
vendored
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
[submodule "third_party/gello_software"]
|
||||||
|
path = third_party/gello_software
|
||||||
|
url = https://github.com/xArm-Developer/gello_software.git
|
||||||
@ -12,9 +12,17 @@ robot:
|
|||||||
teleop:
|
teleop:
|
||||||
type: uf::gello_teleop
|
type: uf::gello_teleop
|
||||||
id: "gello_teleop"
|
id: "gello_teleop"
|
||||||
port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0"
|
port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0"
|
||||||
# GELLO calibration reference; matches the xArm SDK initial point.
|
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]
|
start_joints: [0, -30, 0, 0, 0, 30, 0]
|
||||||
|
gripper_id: 8
|
||||||
|
gripper_open_deg: 198.017578
|
||||||
|
gripper_close_deg: 155.75
|
||||||
|
reset_speed_deg_s: 10.0
|
||||||
|
|
||||||
dataset:
|
dataset:
|
||||||
# root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
|
# root of local repo: /home/<user_name>/.cache/huggingface/lerobot (default)
|
||||||
|
|||||||
37
config/gello/xarm7_gello_reset_calibration.yaml
Normal file
37
config/gello/xarm7_gello_reset_calibration.yaml
Normal file
@ -0,0 +1,37 @@
|
|||||||
|
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
|
||||||
@ -45,8 +45,8 @@ uf-camera-test = "lerobot_robot_ufactory.scripts.uf_camera_test:main"
|
|||||||
[project.optional-dependencies]
|
[project.optional-dependencies]
|
||||||
# GELLO 遥操作
|
# GELLO 遥操作
|
||||||
gello = [
|
gello = [
|
||||||
"gello @ git+https://github.com/xArm-Developer/gello_software.git",
|
"gello",
|
||||||
"dynamixel-sdk @ git+https://github.com/ROBOTIS-GIT/DynamixelSDK.git#subdirectory=python",
|
"dynamixel-sdk>=4.0.5",
|
||||||
]
|
]
|
||||||
# SpaceMouse 遥操作
|
# SpaceMouse 遥操作
|
||||||
spacemouse = [
|
spacemouse = [
|
||||||
@ -70,6 +70,9 @@ where = ["src"]
|
|||||||
[tool.setuptools.package-data]
|
[tool.setuptools.package-data]
|
||||||
"lerobot_robot_ufactory.devices.umi.xvlib" = ["*.so", "*.so.*"]
|
"lerobot_robot_ufactory.devices.umi.xvlib" = ["*.so", "*.so.*"]
|
||||||
|
|
||||||
|
[tool.uv.sources]
|
||||||
|
gello = { path = "third_party/gello_software", editable = true }
|
||||||
|
|
||||||
[tool.ruff]
|
[tool.ruff]
|
||||||
line-length = 100
|
line-length = 100
|
||||||
target-version = "py310"
|
target-version = "py310"
|
||||||
|
|||||||
339
scripts/reset_gello_xarm7.py
Normal file
339
scripts/reset_gello_xarm7.py
Normal file
@ -0,0 +1,339 @@
|
|||||||
|
#!/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,5 +1,6 @@
|
|||||||
import sys
|
import sys
|
||||||
import argparse
|
import argparse
|
||||||
|
import atexit
|
||||||
import logging
|
import logging
|
||||||
import time
|
import time
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
@ -53,8 +54,37 @@ def teleop_loop(cfg: TeleopConfig):
|
|||||||
|
|
||||||
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
|
teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
|
||||||
|
|
||||||
|
robot_connected = False
|
||||||
|
teleop_connected = False
|
||||||
|
listener = None
|
||||||
|
cleanup_done = False
|
||||||
|
|
||||||
|
def cleanup_connections():
|
||||||
|
nonlocal cleanup_done
|
||||||
|
if cleanup_done:
|
||||||
|
return
|
||||||
|
cleanup_done = True
|
||||||
|
if teleop_connected:
|
||||||
|
try:
|
||||||
|
teleop.disconnect()
|
||||||
|
except Exception:
|
||||||
|
logging.exception("Failed to disconnect teleoperator cleanly")
|
||||||
|
if robot_connected:
|
||||||
|
try:
|
||||||
|
robot.disconnect()
|
||||||
|
except Exception:
|
||||||
|
logging.exception("Failed to disconnect robot cleanly")
|
||||||
|
if listener is not None:
|
||||||
|
try:
|
||||||
|
listener.stop()
|
||||||
|
except Exception:
|
||||||
|
logging.exception("Failed to stop keyboard listener cleanly")
|
||||||
|
|
||||||
|
atexit.register(cleanup_connections)
|
||||||
robot.connect()
|
robot.connect()
|
||||||
|
robot_connected = True
|
||||||
teleop.connect()
|
teleop.connect()
|
||||||
|
teleop_connected = True
|
||||||
|
|
||||||
sleep_time_s = 1 / cfg.fps
|
sleep_time_s = 1 / cfg.fps
|
||||||
|
|
||||||
@ -77,7 +107,6 @@ def teleop_loop(cfg: TeleopConfig):
|
|||||||
is_reset = is_uf_teleop
|
is_reset = is_uf_teleop
|
||||||
is_paused = True
|
is_paused = True
|
||||||
events = {"exit": False}
|
events = {"exit": False}
|
||||||
listener = None
|
|
||||||
key_dict = {}
|
key_dict = {}
|
||||||
|
|
||||||
if is_evt:
|
if is_evt:
|
||||||
@ -185,10 +214,8 @@ def teleop_loop(cfg: TeleopConfig):
|
|||||||
precise_sleep(sleep_time_s - dt_s)
|
precise_sleep(sleep_time_s - dt_s)
|
||||||
|
|
||||||
print("\n********** Teleop Control Loop Exit **********")
|
print("\n********** Teleop Control Loop Exit **********")
|
||||||
robot.disconnect()
|
cleanup_connections()
|
||||||
teleop.disconnect()
|
atexit.unregister(cleanup_connections)
|
||||||
if is_evt and listener is not None:
|
|
||||||
listener.stop()
|
|
||||||
|
|
||||||
@parser.wrap()
|
@parser.wrap()
|
||||||
def get_cfg(cfg: TeleopConfig) -> TeleopConfig:
|
def get_cfg(cfg: TeleopConfig) -> TeleopConfig:
|
||||||
|
|||||||
@ -10,7 +10,6 @@ from .gello_teleop_config import GelloTeleopConfig
|
|||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
GELLO_RESET_SPEED_DEG = 30.0
|
|
||||||
GELLO_RESET_TOLERANCE_DEG = 2.0
|
GELLO_RESET_TOLERANCE_DEG = 2.0
|
||||||
GELLO_RESET_CONTROL_HZ = 50.0
|
GELLO_RESET_CONTROL_HZ = 50.0
|
||||||
GELLO_RESET_TIMEOUT_MARGIN_S = 5.0
|
GELLO_RESET_TIMEOUT_MARGIN_S = 5.0
|
||||||
@ -44,13 +43,26 @@ class GelloTeleop(UFBaseTeleop):
|
|||||||
driver.get_joints() # warmup
|
driver.get_joints() # warmup
|
||||||
curr_joints = driver.get_joints()
|
curr_joints = driver.get_joints()
|
||||||
driver.close()
|
driver.close()
|
||||||
joint_offsets = []
|
|
||||||
start_joints = list(map(math.radians, self.config.start_joints))
|
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)):
|
for i in range(len(start_joints)):
|
||||||
offset = curr_joints[i] - start_joints[i] / self.config.joint_signs[i]
|
offset = curr_joints[i] - start_joints[i] / self.config.joint_signs[i]
|
||||||
joint_offsets.append(offset)
|
joint_offsets.append(offset)
|
||||||
if self.config.gripper_id >= 0:
|
if self.config.gripper_id >= 0:
|
||||||
gripper_config = [self.config.gripper_id, np.rad2deg(curr_joints[-1]) - 0.2, np.rad2deg(curr_joints[-1]) - 42]
|
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
|
||||||
|
gripper_config = [
|
||||||
|
self.config.gripper_id,
|
||||||
|
gripper_open_deg,
|
||||||
|
gripper_close_deg,
|
||||||
|
]
|
||||||
else:
|
else:
|
||||||
gripper_config = None
|
gripper_config = None
|
||||||
|
|
||||||
@ -150,7 +162,7 @@ class GelloTeleop(UFBaseTeleop):
|
|||||||
target_raw[self.dof] = gripper_open + gripper_pos * (gripper_close - gripper_open)
|
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]))
|
arm_delta = np.max(np.abs(target_raw[: self.dof] - current_raw[: self.dof]))
|
||||||
reset_speed_rad_s = math.radians(GELLO_RESET_SPEED_DEG)
|
reset_speed_rad_s = math.radians(self.config.reset_speed_deg_s)
|
||||||
duration_s = max(0.5, float(arm_delta / reset_speed_rad_s))
|
duration_s = max(0.5, float(arm_delta / reset_speed_rad_s))
|
||||||
deadline = time.perf_counter() + duration_s + GELLO_RESET_TIMEOUT_MARGIN_S
|
deadline = time.perf_counter() + duration_s + GELLO_RESET_TIMEOUT_MARGIN_S
|
||||||
success = False
|
success = False
|
||||||
|
|||||||
@ -1,7 +1,7 @@
|
|||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from typing import Tuple
|
from typing import Optional, Tuple
|
||||||
from lerobot.teleoperators import TeleoperatorConfig
|
from lerobot.teleoperators import TeleoperatorConfig
|
||||||
|
|
||||||
|
|
||||||
@ -14,10 +14,26 @@ class GelloTeleopConfig(TeleoperatorConfig):
|
|||||||
# Others: Calibration angles, joint directions etc
|
# Others: Calibration angles, joint directions etc
|
||||||
joint_ids: Tuple[int, ...] = (1, 2, 3, 4, 5, 6, 7)
|
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
|
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.
|
||||||
|
joint_offsets: Optional[Tuple[float, ...]] = None
|
||||||
# GELLO encoder calibration reference; this is not the xArm reset target.
|
# GELLO encoder calibration reference; this is not the xArm reset target.
|
||||||
start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # °
|
start_joints: Tuple[float, ...] = (0, 0, 0, 90, 0, 90, 0) # °
|
||||||
gripper_id: int = 8 # -1: no gripper
|
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; reset controls all GELLO joints.
|
||||||
|
|
||||||
def __post_init__(self):
|
def __post_init__(self):
|
||||||
self.id = 'gello_teleop' if self.id is None else self.id
|
self.id = 'gello_teleop' if self.id is None else self.id
|
||||||
|
if len(self.joint_ids) != len(self.joint_signs):
|
||||||
|
raise ValueError("joint_ids and joint_signs must have the same length")
|
||||||
|
if len(self.joint_ids) != len(self.start_joints):
|
||||||
|
raise ValueError("joint_ids and start_joints must have the same length")
|
||||||
|
if self.joint_offsets is not None and len(self.joint_ids) != len(self.joint_offsets):
|
||||||
|
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")
|
||||||
|
|||||||
1
third_party/gello_software
vendored
Submodule
1
third_party/gello_software
vendored
Submodule
@ -0,0 +1 @@
|
|||||||
|
Subproject commit b543065ca36f7a444f7f5d110e49b8146d7e1cf3
|
||||||
15
uv.lock
generated
15
uv.lock
generated
@ -743,10 +743,14 @@ wheels = [
|
|||||||
[[package]]
|
[[package]]
|
||||||
name = "dynamixel-sdk"
|
name = "dynamixel-sdk"
|
||||||
version = "4.0.5"
|
version = "4.0.5"
|
||||||
source = { git = "https://github.com/ROBOTIS-GIT/DynamixelSDK.git?subdirectory=python#2ded684dff05a40ac78d6a16105c6ddc1b3b9930" }
|
source = { registry = "https://pypi.org/simple" }
|
||||||
dependencies = [
|
dependencies = [
|
||||||
{ name = "pyserial" },
|
{ name = "pyserial" },
|
||||||
]
|
]
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/de/ad/05bb6c7fe54c01d2712398872b300891a5b6a0181e69335f4e1717d72805/dynamixel_sdk-4.0.5.tar.gz", hash = "sha256:498ba2090f5f9844ac0610553cc70b8c79e3f6f52f7911425cdb2857210b9630", size = 29695, upload-time = "2026-05-06T02:12:08.389Z" }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/52/a5/319d15afd31997e54e5c88b2fe1d53d15c9e63c3b8d51eda40ddff629443/dynamixel_sdk-4.0.5-py3-none-any.whl", hash = "sha256:36f9c0c078cbb8e87f5413bfcf76da8f50ce07d17690c52e52ad0f0180a7d6d8", size = 103493, upload-time = "2026-05-06T02:12:06.77Z" },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "einops"
|
name = "einops"
|
||||||
@ -931,13 +935,16 @@ http = [
|
|||||||
[[package]]
|
[[package]]
|
||||||
name = "gello"
|
name = "gello"
|
||||||
version = "0.0.1"
|
version = "0.0.1"
|
||||||
source = { git = "https://github.com/xArm-Developer/gello_software.git#523437fa4615155813124efeae26fdef32e546a0" }
|
source = { editable = "third_party/gello_software" }
|
||||||
dependencies = [
|
dependencies = [
|
||||||
{ name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" },
|
{ 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.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'" },
|
{ 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]]
|
[[package]]
|
||||||
name = "gitdb"
|
name = "gitdb"
|
||||||
version = "4.0.12"
|
version = "4.0.12"
|
||||||
@ -1246,8 +1253,8 @@ spacemouse = [
|
|||||||
|
|
||||||
[package.metadata]
|
[package.metadata]
|
||||||
requires-dist = [
|
requires-dist = [
|
||||||
{ name = "dynamixel-sdk", marker = "extra == 'gello'", git = "https://github.com/ROBOTIS-GIT/DynamixelSDK.git?subdirectory=python" },
|
{ name = "dynamixel-sdk", marker = "extra == 'gello'", specifier = ">=4.0.5" },
|
||||||
{ name = "gello", marker = "extra == 'gello'", git = "https://github.com/xArm-Developer/gello_software.git" },
|
{ name = "gello", marker = "extra == 'gello'", editable = "third_party/gello_software" },
|
||||||
{ name = "lerobot", extras = ["intelrealsense"], specifier = "==0.4.3" },
|
{ name = "lerobot", extras = ["intelrealsense"], specifier = "==0.4.3" },
|
||||||
{ name = "mypy", marker = "extra == 'dev'", specifier = ">=1.10" },
|
{ name = "mypy", marker = "extra == 'dev'", specifier = ">=1.10" },
|
||||||
{ name = "numpy", specifier = ">=1.24" },
|
{ name = "numpy", specifier = ">=1.24" },
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user