mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-23 01:41:54 +00:00
feat(ax-arm): add AX-series arm robot package
Add the lerobot_robot_ax_arm package with the AXArm robot, its config, URDF joint mapping, an end-effector keyboard teleop example, and packaging metadata.
This commit is contained in:
CarolinePascal
@@ -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()
|
||||
```
|
||||
@@ -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()
|
||||
@@ -0,0 +1,4 @@
|
||||
from .ax_arm import AXArm
|
||||
from .config_ax_arm import AXArmConfig
|
||||
|
||||
__all__ = ["AXArm", "AXArmConfig"]
|
||||
@@ -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.")
|
||||
@@ -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
|
||||
@@ -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)}
|
||||
@@ -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"]
|
||||
Reference in New Issue
Block a user