diff --git a/lerobot_robot_ax_arm/README.md b/lerobot_robot_ax_arm/README.md new file mode 100644 index 000000000..5d234c237 --- /dev/null +++ b/lerobot_robot_ax_arm/README.md @@ -0,0 +1,49 @@ +# lerobot_robot_ax_arm + +A third-party [LeRobot](https://github.com/huggingface/lerobot) robot: a 4-DoF arm driven by Dynamixel +AX-series servos (e.g. AX-12A) over **Protocol 1.0**. + +| Motor ID | Joint | Normalization | +| -------- | --------------- | ------------- | +| 1 | `shoulder_pan` | [-100, 100] | +| 2 | `shoulder_lift` | [-100, 100] | +| 3 | `elbow_flex` | [-100, 100] | +| 4 | `gripper` | [0, 100] | + +## Protocol 1.0 notes + +AX-series motors differ from the X-series (Protocol 2.0) used elsewhere in LeRobot: + +- **No Sync Read** — positions are read sequentially (`get_observation` / calibration). Sync Write is still + used for `Goal_Position`. +- **No `Operating_Mode` / PID registers** — `configure()` only lowers the return delay time. +- **No homing offset register** — calibration records the range of motion only (`homing_offset = 0`) and is + stored via the CW/CCW angle limits. + +## Install + +```bash +pip install -e . +``` + +This is discovered automatically by LeRobot thanks to the `lerobot_robot_` package prefix. + +## Usage + +```bash +lerobot-record \ + --robot.type=ax_arm \ + --robot.port=/dev/tty.usbserial-AL02L1E0 \ + # ... other arguments +``` + +Or from Python: + +```python +from lerobot_robot_ax_arm import AXArm, AXArmConfig + +robot = AXArm(AXArmConfig(port="/dev/tty.usbserial-AL02L1E0")) +robot.connect() +obs = robot.get_observation() +robot.disconnect() +``` diff --git a/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py b/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py new file mode 100644 index 000000000..ed35baa9c --- /dev/null +++ b/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python + +"""Keyboard end-effector teleoperation for the 4-DoF AX arm (position-only IK on the URDF). + +The arm has 3 revolute joints (base yaw + shoulder/elbow pitch) giving exactly 3 task-space DoF, +all spent on reaching a 3D *position* (orientation is not controllable). Each frame we: + + 1. read the raw motor ticks and map them to URDF joint degrees (see ``urdf_mapping``), + 2. forward-kinematics to the current end-effector pose, + 3. offset the target position by the pressed keys, + 4. solve position-only IK (``orientation_weight=0.0``) for new URDF joint degrees, + 5. map back to ticks and command the servos. + +The tick<->URDF mapping is established once by ``lerobot-calibrate`` (reference pose + travel +limits), so no separate alignment step is needed here. + +Controls (letter keys; hold to keep moving via terminal key-repeat): + - w / s : +X / -X (forward / back) + - a / d : +Y / -Y (left / right) + - r / f : +Z / -Z (up / down) + - o / c : open / close gripper + - ESC / q : stop + +Run: + python examples/teleoperate_ee_keyboard.py --port /dev/tty.usbserial-XXXX --id my_ax_arm +""" + +import argparse +import time +from importlib.resources import files + +import numpy as np + +from lerobot.model.kinematics import RobotKinematics +from lerobot.utils.keyboard_input import create_key_listener +from lerobot.utils.robot_utils import precise_sleep + +from lerobot_robot_ax_arm import AXArm, AXArmConfig +from lerobot_robot_ax_arm.urdf_mapping import ( + ARM_JOINTS, + URDF_JOINT_NAMES, + ticks_to_urdf_vector, + urdf_vector_to_ticks, +) + +FPS = 30 +LINEAR_STEP_M = 0.005 # EE position change per pressed frame +GRIP_STEP_TICK = 15 # gripper ticks per press + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--port", required=True, help="Serial port of the AX arm") + parser.add_argument("--id", default="my_ax_arm", help="Robot id used for calibration files") + args = parser.parse_args() + + robot = AXArm(AXArmConfig(port=args.port, id=args.id, use_degrees=True)) + robot.connect(calibrate=False) + if not robot.calibration: + raise RuntimeError(f"No calibration found for id '{args.id}'. Run lerobot-calibrate first.") + + urdf_path = str(files("lerobot_robot_ax_arm") / "urdf" / "ax_arm.urdf") + kin = RobotKinematics(urdf_path, target_frame_name="gripper_link", joint_names=URDF_JOINT_NAMES) + + grip_calib = robot.calibration["gripper"] + grip_tick = int(robot.bus.read("Present_Position", "gripper", normalize=False)) + + pending = {"x": 0.0, "y": 0.0, "z": 0.0, "g": 0.0} + state = {"quit": False} + keymap = {"w": ("x", 1), "s": ("x", -1), "a": ("y", 1), "d": ("y", -1), "r": ("z", 1), "f": ("z", -1), + "o": ("g", 1), "c": ("g", -1)} + + def on_key(name: str) -> None: + k = name.lower() + if k in ("esc", "q"): + state["quit"] = True + elif k in keymap: + axis, direction = keymap[k] + pending[axis] += direction + + listener = create_key_listener(on_key, controls_help="w/s a/d r/f = XYZ, o/c = gripper, esc = stop") + if listener is None: + raise RuntimeError("Needs an interactive terminal with a usable key listener.") + + print("Keyboard EE teleop. w/s=X a/d=Y r/f=Z o/c=gripper, ESC=stop.") + try: + while not state["quit"]: + t0 = time.perf_counter() + + ticks = {j: float(robot.bus.read("Present_Position", j, normalize=False)) for j in ARM_JOINTS} + q_deg = ticks_to_urdf_vector(ticks, robot.calibration) + + pose = kin.forward_kinematics(q_deg) + dx, dy, dz = (np.sign(pending[a]) for a in ("x", "y", "z")) + pending["x"] = pending["y"] = pending["z"] = 0.0 + pose[:3, 3] += LINEAR_STEP_M * np.array([dx, dy, dz]) + + q_target = kin.inverse_kinematics(q_deg, pose, orientation_weight=0.0) + target_ticks = urdf_vector_to_ticks(q_target, robot.calibration) + for j in ARM_JOINTS: + c = robot.calibration[j] + tick = int(np.clip(target_ticks[j], c.range_min, c.range_max)) + robot.bus.write("Goal_Position", j, tick, normalize=False) + + g = np.sign(pending["g"]) + pending["g"] = 0.0 + if g: + grip_tick = int(np.clip(grip_tick + g * GRIP_STEP_TICK, grip_calib.range_min, grip_calib.range_max)) + robot.bus.write("Goal_Position", "gripper", grip_tick, normalize=False) + + urdf_str = " ".join(f"{j}={v:+6.1f}" for j, v in zip(ARM_JOINTS, q_target)) + print(f"cmd[x={dx:+.0f} y={dy:+.0f} z={dz:+.0f} g={g:+.0f}] -> urdf[{urdf_str}] gripper={grip_tick}", + end="\r", flush=True) + + precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0)) + except KeyboardInterrupt: + pass + finally: + listener.stop() + print() + robot.disconnect() + + +if __name__ == "__main__": + main() diff --git a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/__init__.py b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/__init__.py new file mode 100644 index 000000000..173f4badc --- /dev/null +++ b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/__init__.py @@ -0,0 +1,4 @@ +from .ax_arm import AXArm +from .config_ax_arm import AXArmConfig + +__all__ = ["AXArm", "AXArmConfig"] diff --git a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py new file mode 100644 index 000000000..bd6bf190e --- /dev/null +++ b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py @@ -0,0 +1,260 @@ +import logging +import time +from functools import cached_property + +from lerobot.cameras import make_cameras_from_configs +from lerobot.motors import Motor, MotorCalibration, MotorNormMode +from lerobot.motors.dynamixel import DynamixelMotorsBus +from lerobot.robots import Robot +from lerobot.robots.utils import ensure_safe_goal_position +from lerobot.types import RobotAction, RobotObservation +from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected +from lerobot.utils.keyboard_input import create_key_listener + +from .config_ax_arm import AXArmConfig +from .urdf_mapping import ARM_JOINTS, REFERENCE_URDF_DEG, URDF_LIMITS_DEG + +logger = logging.getLogger(__name__) + + +class AXArm(Robot): + """A 4-DoF arm driven by Dynamixel AX-series servos over Protocol 1.0. + + Protocol 1.0 has no Sync Read, no Operating_Mode register and no homing offset, so this robot reads + positions sequentially and calibrates by recording the range of motion only. + """ + + config_class = AXArmConfig + name = "ax_arm" + + def __init__(self, config: AXArmConfig): + super().__init__(config) + self.config = config + norm_mode_body = MotorNormMode.DEGREES if config.use_degrees else MotorNormMode.RANGE_M100_100 + self.bus = DynamixelMotorsBus( + port=self.config.port, + # NOTE: dict order (pan, lift, elbow, gripper) must stay aligned with the URDF joints + # (robot_joint_1/2/3) and keep the gripper last for the kinematics pipeline. Only the + # motor IDs below reflect the physical bus wiring. + motors={ + "shoulder_pan": Motor(3, "ax-12a", norm_mode_body), + "shoulder_lift": Motor(4, "ax-12a", norm_mode_body), + "elbow_flex": Motor(2, "ax-12a", norm_mode_body), + "gripper": Motor(1, "ax-12a", MotorNormMode.RANGE_0_100), + }, + calibration=self.calibration, + protocol_version=1, + ) + self.cameras = make_cameras_from_configs(config.cameras) + + @property + def _motors_ft(self) -> dict[str, type]: + return {f"{motor}.pos": float for motor in self.bus.motors} + + @property + def _cameras_ft(self) -> dict[str, tuple]: + return {cam: (self.cameras[cam].height, self.cameras[cam].width, 3) for cam in self.cameras} + + @cached_property + def observation_features(self) -> dict[str, type | tuple]: + return {**self._motors_ft, **self._cameras_ft} + + @cached_property + def action_features(self) -> dict[str, type]: + return self._motors_ft + + @property + def is_connected(self) -> bool: + return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) + + @check_if_already_connected + def connect(self, calibrate: bool = True) -> None: + self.bus.connect() + if not self.is_calibrated and calibrate: + logger.info("No matching calibration found, running calibration.") + self.calibrate() + + for cam in self.cameras.values(): + cam.connect() + + self.configure() + logger.info(f"{self} connected.") + + @property + def is_calibrated(self) -> bool: + # Protocol 1.0 cannot read back homing_offset/drive_mode (we repurpose them to store the + # URDF mapping: tick_ref and sign), so only the range limits are actually stored on the + # servos. Compare just those to decide whether the on-file calibration matches the hardware. + if not self.calibration: + return False + hw = self.bus.read_calibration() + return all( + self.calibration[m].range_min == hw[m].range_min + and self.calibration[m].range_max == hw[m].range_max + for m in self.bus.motors + ) + + def calibrate(self) -> None: + if self.calibration: + user_input = input( + f"Press ENTER to use provided calibration file associated with the id {self.id}, or type 'c' and press ENTER to run calibration: " + ) + if user_input.strip().lower() != "c": + logger.info(f"Writing calibration file associated with the id {self.id} to the motors") + self.bus.write_calibration(self.calibration) + return + + logger.info(f"\nRunning calibration of {self}") + + # This arm cannot be backdriven with torque off, so each joint is jogged with the keyboard + # (torque on). Arm joints capture a reference pose (known URDF angle) plus the lower/upper + # travel limits; the reference tick and mounting sign encode the URDF mapping (see urdf_mapping). + captured = self._record_calibration() + + self.calibration = {} + for motor, m in self.bus.motors.items(): + c = captured[motor] + if motor in ARM_JOINTS: + lower, upper = c["lower limit"], c["upper limit"] + # sign: +1 if jogging toward the URDF-upper limit increased ticks, else -1. + drive_mode = 0 if upper >= lower else 1 + self.calibration[motor] = MotorCalibration( + id=m.id, + drive_mode=drive_mode, + homing_offset=c["reference"], # tick at the URDF reference pose + range_min=min(lower, upper), + range_max=max(lower, upper), + ) + else: # gripper: plain range, no URDF mapping + lo, hi = c["min"], c["max"] + self.calibration[motor] = MotorCalibration( + id=m.id, drive_mode=0, homing_offset=0, range_min=min(lo, hi), range_max=max(lo, hi) + ) + + self.bus.write_calibration(self.calibration) + self._save_calibration() + print("Calibration saved to", self.calibration_fpath) + + # Raw ticks the selected joint moves per keypress (~3° on an AX-12A). + _JOG_STEP = 10 + + def _capture_plan(self) -> list[tuple[str, str, float | None]]: + """Ordered (motor, label, target_urdf_deg) captures. Arm joints: reference + lower/upper + URDF limits; gripper: plain min/max. ``target_urdf_deg`` is None when there is no URDF angle.""" + plan: list[tuple[str, str, float | None]] = [] + for motor in self.bus.motors: + if motor in ARM_JOINTS: + lower, upper = URDF_LIMITS_DEG[motor] + plan.append((motor, "reference", REFERENCE_URDF_DEG[motor])) + plan.append((motor, "lower limit", lower)) + plan.append((motor, "upper limit", upper)) + else: + plan += [(motor, "min", None), (motor, "max", None)] + return plan + + def _record_calibration(self) -> dict[str, dict[str, int]]: + # Protocol 1.0 has no Sync Read, so positions are read sequentially, one motor at a time. + motor_names = list(self.bus.motors) + max_pos = min(self.bus.model_resolution_table[m.model] for m in self.bus.motors.values()) - 1 + + # Temporarily widen the angle limits to full travel so pre-existing (possibly narrow) limits do + # not cap the reachable range; write_calibration() sets them to the recorded min/max afterwards. + for motor in motor_names: + self.bus.write("CW_Angle_Limit", motor, 0) + self.bus.write("CCW_Angle_Limit", motor, max_pos) + self.bus.enable_torque() + + plan = self._capture_plan() + captured: dict[str, dict[str, int]] = {motor: {} for motor in motor_names} + state = {"i": 0, "target": 0, "capture": False, "done": False} + state["target"] = int(self.bus.read("Present_Position", plan[0][0], normalize=False)) + + def on_key(name: str) -> None: + key = name.lower() + if key == "up": + state["target"] = min(max_pos, state["target"] + self._JOG_STEP) + elif key == "down": + state["target"] = max(0, state["target"] - self._JOG_STEP) + elif key == "enter": + state["capture"] = True + + listener = create_key_listener(on_key, controls_help="up/down=jog, enter=capture") + if listener is None: + raise RuntimeError( + "Keyboard calibration requires an interactive terminal with a usable key listener." + ) + + print( + "Jog each joint to the requested pose and press ENTER to capture it.\n" + " up/down = jog | enter = capture" + ) + try: + while not state["done"]: + motor, label, target_deg = plan[state["i"]] + self.bus.write("Goal_Position", motor, state["target"], normalize=False) + pos = int(self.bus.read("Present_Position", motor, normalize=False)) + hint = f" (URDF {target_deg:+.0f} deg)" if target_deg is not None else "" + print( + f" [{state['i'] + 1}/{len(plan)}] jog '{motor}' to {label}{hint}: {pos:4d} tick ", + end="\r", + flush=True, + ) + + if state["capture"]: + state["capture"] = False + captured[motor][label] = pos + print(f" [{state['i'] + 1}/{len(plan)}] {motor} {label}{hint} = {pos} tick") + state["i"] += 1 + state["done"] = state["i"] >= len(plan) + if not state["done"]: + state["target"] = int( + self.bus.read("Present_Position", plan[state["i"]][0], normalize=False) + ) + time.sleep(0.02) + finally: + listener.stop() + print() + + for motor, c in captured.items(): + if len(set(c.values())) < len(c): + raise ValueError(f"Motor '{motor}' has duplicate captured ticks: {c}") + return captured + + def configure(self) -> None: + # AX-series has no Operating_Mode/PID registers; configure_motors only lowers the return delay time. + with self.bus.torque_disabled(): + self.bus.configure_motors() + + @check_if_not_connected + def get_observation(self) -> RobotObservation: + start = time.perf_counter() + # Protocol 1.0 has no Sync Read, so read each motor sequentially. + obs_dict = {f"{motor}.pos": self.bus.read("Present_Position", motor) for motor in self.bus.motors} + dt_ms = (time.perf_counter() - start) * 1e3 + logger.debug(f"{self} read state: {dt_ms:.1f}ms") + + for cam_key, cam in self.cameras.items(): + obs_dict[cam_key] = cam.async_read() + + return obs_dict + + @check_if_not_connected + def send_action(self, action: RobotAction) -> RobotAction: + goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} + + if self.config.max_relative_target is not None: + present_pos = {motor: self.bus.read("Present_Position", motor) for motor in goal_pos} + goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()} + goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) + + # Sync Write is available on Protocol 1.0 (only Sync Read and Broadcast Ping are not). + self.bus.sync_write("Goal_Position", goal_pos) + return {f"{motor}.pos": val for motor, val in goal_pos.items()} + + @check_if_not_connected + def disconnect(self) -> None: + self.bus.disconnect(self.config.disable_torque_on_disconnect) + for cam in self.cameras.values(): + cam.disconnect() + + logger.info(f"{self} disconnected.") diff --git a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/config_ax_arm.py b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/config_ax_arm.py new file mode 100644 index 000000000..a9a5ceb68 --- /dev/null +++ b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/config_ax_arm.py @@ -0,0 +1,24 @@ +from dataclasses import dataclass, field + +from lerobot.cameras import CameraConfig +from lerobot.robots import RobotConfig + + +@RobotConfig.register_subclass("ax_arm") +@dataclass +class AXArmConfig(RobotConfig): + """Configuration for a 4-DoF Dynamixel AX-series (Protocol 1.0) arm.""" + + # Serial port the arm is connected to (e.g. "/dev/tty.usbserial-AL02L1E0"). + port: str + + disable_torque_on_disconnect: bool = True + + # Limits the magnitude of relative positional targets for safety. Scalar (same for all motors) or a + # dict mapping motor names to their own cap. + max_relative_target: float | dict[str, float] | None = None + + cameras: dict[str, CameraConfig] = field(default_factory=dict) + + # Normalize body joints to [-100, 100] (and gripper to [0, 100]) when False, or to degrees when True. + use_degrees: bool = False diff --git a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py new file mode 100644 index 000000000..73a56c7f3 --- /dev/null +++ b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py @@ -0,0 +1,61 @@ +"""Mapping between AX motor ticks and the URDF joint convention. + +The AX-12A reports raw ticks (0-1023) over ~300 deg of travel, with a zero and direction +unrelated to the URDF. We map ticks <-> URDF joint degrees with a per-joint affine transform +anchored at a reference pose captured during calibration:: + + q_urdf_deg = q_ref + sign * (tick - tick_ref) * SCALE + +- ``SCALE`` : AX-12A mechanical travel per tick (300 deg / 1023 ticks). +- ``q_ref`` : URDF angle of the reference pose (:data:`REFERENCE_URDF_DEG`). +- ``tick_ref`` : tick captured at that reference pose, stored as ``MotorCalibration.homing_offset``. +- ``sign`` : mounting direction (+1/-1), stored as ``MotorCalibration.drive_mode`` (0 -> +1, 1 -> -1). + +The ``homing_offset``/``drive_mode`` fields are unused by the Dynamixel Protocol-1.0 normalization +path, so repurposing them here does not affect anything else. +""" + +from __future__ import annotations + +import numpy as np + +from lerobot.motors import MotorCalibration + +# Arm joints in URDF order (base yaw, shoulder pitch, elbow pitch). The gripper is not remapped. +ARM_JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex") +URDF_JOINT_NAMES = ["robot_joint_1", "robot_joint_2", "robot_joint_3"] + +AX_TRAVEL_DEG = 300.0 # AX-12A mechanical travel over the full 0-1023 tick range +AX_MAX_TICK = 1023.0 +SCALE = AX_TRAVEL_DEG / AX_MAX_TICK # URDF degrees per motor tick + +# URDF joint angle (deg) of each arm joint at the calibration reference pose. +REFERENCE_URDF_DEG = {"shoulder_pan": 0.0, "shoulder_lift": 45.0, "elbow_flex": 90.0} +# URDF joint limits (deg) from ax_arm.urdf, used to guide the lower/upper jog during calibration. +URDF_LIMITS_DEG = { + "shoulder_pan": (-45.0, 45.0), + "shoulder_lift": (0.0, 90.0), + "elbow_flex": (0.0, 90.0), +} + + +def _sign(calib: MotorCalibration) -> float: + return -1.0 if calib.drive_mode else 1.0 + + +def tick_to_urdf_deg(joint: str, tick: float, calib: MotorCalibration) -> float: + return REFERENCE_URDF_DEG[joint] + _sign(calib) * (tick - calib.homing_offset) * SCALE + + +def urdf_deg_to_tick(joint: str, q_deg: float, calib: MotorCalibration) -> int: + return int(round(calib.homing_offset + _sign(calib) * (q_deg - REFERENCE_URDF_DEG[joint]) / SCALE)) + + +def ticks_to_urdf_vector(ticks: dict[str, float], calibration: dict[str, MotorCalibration]) -> np.ndarray: + """Arm joint ticks -> URDF joint angles (deg), in :data:`ARM_JOINTS` order.""" + return np.array([tick_to_urdf_deg(j, ticks[j], calibration[j]) for j in ARM_JOINTS], dtype=float) + + +def urdf_vector_to_ticks(q_deg: np.ndarray, calibration: dict[str, MotorCalibration]) -> dict[str, int]: + """URDF joint angles (deg), in :data:`ARM_JOINTS` order -> arm joint ticks.""" + return {j: urdf_deg_to_tick(j, float(q_deg[i]), calibration[j]) for i, j in enumerate(ARM_JOINTS)} diff --git a/lerobot_robot_ax_arm/pyproject.toml b/lerobot_robot_ax_arm/pyproject.toml new file mode 100644 index 000000000..cfd2a3f74 --- /dev/null +++ b/lerobot_robot_ax_arm/pyproject.toml @@ -0,0 +1,19 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "lerobot_robot_ax_arm" +version = "0.1.0" +description = "Third-party LeRobot robot: 4-DoF Dynamixel AX-series (Protocol 1.0) arm" +readme = "README.md" +requires-python = ">=3.10" +dependencies = [ + "lerobot[dynamixel]", +] + +[tool.setuptools.packages.find] +include = ["lerobot_robot_ax_arm*"] + +[tool.setuptools.package-data] +lerobot_robot_ax_arm = ["urdf/*.urdf"]