Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python

arm in dimOS: the moss-arm blueprint (the sim adapter on the CAD scene, the Feetech adapter on the robot), the cycle as one JointTrajectory, the executor with its following-error watch, two cold benches

metroxbfc5079549b8
diff --git a/benches/bench_moss_arm_feetech_cold.py b/benches/bench_moss_arm_feetech_cold.py
new file mode 100644
index 0000000..9c4c897
--- /dev/null
+++ b/benches/bench_moss_arm_feetech_cold.py
@@ -0,0 +1,323 @@
+"""Cold bench for moss_dimos.arm_feetech: the SO-101's servos as a dimOS
+adapter, on a fake bus, no robot.
+
+Known input -> known output in physical units. Groups:
+
+ A. units - rad <-> ticks through the joint map (2048 = 0, +30 deg =
+ 2389 ticks, the drive pose's 860 = -104.4 deg), the gripper's
+ line (90 mm = 3674, 53 mm = 2780, 24.2 ticks/mm, clamped)
+ B. connect - the adapter opens the fake bus, reads the six servos, sets
+ speed 400 and acceleration 50 on the five arm servos, the
+ force 400 on the gripper's registers 44 and 48, writes no
+ torque; activate turns torque on and holds the present
+ position; limits = the calibration's end stops in radians
+ C. follow - a goal of +30 deg on the pan writes 2389 to register 42 of
+ servo 1 and the fake servo reaches it at 400 ticks/s: read
+ back +30.0 deg; the gripper to 53 mm writes 2780 and reads
+ 53 mm; a repeated goal writes nothing; velocities come from
+ the reads
+ D. watch - a blocked shoulder (the fake servo stops 338 ticks short)
+ raises FOLLOWING_ERROR after lag_s once it stands still,
+ naming the joint and the ticks, not while it is on its way
+ to a far goal; the error clears when the servo arrives; a bus
+ that stops answering leaves the last positions and turns
+ has_live_state False after stale_s
+ E. stop - write_stop sets every goal to the present position;
+ deactivate turns torque off; disconnect leaves a bus it was
+ handed open (the owner closes it)
+ F. coordinator - the adapter under dimOS's ControlCoordinator in this
+ process (registered as feetech_so101 with the fake bus
+ through adapter_kwargs): a 2 s pan trajectory reads back
+ +30 deg within 1 deg; the trajectory task's start check
+ refuses a trajectory 10 deg off the arm
+
+Run (the dimOS main venv for F; A-E need only the repo):
+ nice -n 10 <dimos venv>/bin/python benches/bench_moss_arm_feetech_cold.py
+Env: MOSS_ARM_CALIB (a calibration file elsewhere than config/arm_calibration.json).
+"""
+
+from __future__ import annotations
+
+import math
+import os
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+if os.nice(0) < 10:
+ os.nice(10 - os.nice(0))
+
+from moss_dimos import arm as A # noqa: E402
+from moss_dimos import arm_feetech as F # noqa: E402
+
+FAIL: list[str] = []
+OK = 0
+DEG = 180.0 / math.pi
+CALIB = os.environ.get("MOSS_ARM_CALIB") or str(A.CALIBRATION_PATH)
+
+
+def check(cond: bool, what: str) -> None:
+ global OK
+ if cond:
+ OK += 1
+ print(f" ok {what}")
+ else:
+ FAIL.append(what)
+ print(f" FAIL {what}")
+
+
+class FakeServo:
+ def __init__(self, pos: int) -> None:
+ self.pos = float(pos)
+ self.goal = int(pos)
+ self.speed = 0
+ self.acc = 0
+ self.torque = 0
+ self.regs: dict[int, int] = {}
+ self.block: float | None = None # the servo cannot pass this tick
+
+ def step(self, dt: float) -> None:
+ if not self.torque:
+ return
+ vmax = float(self.speed or 3000)
+ e = self.goal - self.pos
+ if abs(e) < 0.5:
+ self.pos = float(self.goal)
+ return
+ new = self.pos + math.copysign(min(vmax * dt, abs(e)), e)
+ if self.block is not None and (self.pos - self.block) * (new - self.block) <= 0:
+ new = self.block
+ self.pos = new
+
+
+class FakeBus:
+ """The four calls of FeetechBus on servos that move at their speed, in real time."""
+
+ def __init__(self, start: dict[int, int], dead: bool = False) -> None:
+ self.servos = {sid: FakeServo(p) for sid, p in start.items()}
+ self.writes: list[tuple[int, int, int]] = []
+ self.dead = dead
+ self.last = time.monotonic()
+ self.opened = False
+ self.closed = False
+
+ def _tick(self) -> None:
+ now = time.monotonic()
+ for s in self.servos.values():
+ s.step(now - self.last)
+ self.last = now
+
+ def open(self) -> bool:
+ self.opened = True
+ return True
+
+ def close(self) -> None:
+ self.closed = True
+
+ def read2(self, sid: int, addr: int) -> int | None:
+ self._tick()
+ if self.dead or sid not in self.servos:
+ return None
+ s = self.servos[sid]
+ if addr == F.REG_PRESENT:
+ p = int(round(s.pos))
+ return (abs(p) | 0x8000) if p < 0 else p
+ return s.regs.get(addr, 0)
+
+ def write1(self, sid: int, addr: int, value: int) -> bool:
+ self._tick()
+ if self.dead or sid not in self.servos:
+ return False
+ self.writes.append((sid, addr, int(value)))
+ s = self.servos[sid]
+ if addr == F.REG_TORQUE:
+ s.torque = int(value)
+ elif addr == F.REG_ACC:
+ s.acc = int(value)
+ s.regs[addr] = int(value)
+ return True
+
+ def write2(self, sid: int, addr: int, value: int) -> bool:
+ self._tick()
+ if self.dead or sid not in self.servos:
+ return False
+ self.writes.append((sid, addr, int(value)))
+ s = self.servos[sid]
+ if addr == F.REG_GOAL:
+ s.goal = int(value)
+ elif addr == F.REG_SPEED:
+ s.speed = int(value)
+ s.regs[addr] = int(value)
+ return True
+
+
+DRIVE = {1: 2857, 2: 860, 3: 3206, 4: 868, 5: 3221, 6: 3674}
+
+
+def make(dead: bool = False, **kw):
+ bus = FakeBus(DRIVE, dead=dead)
+ ad = F.FeetechSo101Adapter(dof=6, address="/fake", hardware_id="arm", calibration=CALIB, bus=bus, **kw)
+ return bus, ad
+
+
+# ── A. units ──
+print("A. units")
+bus, ad = make()
+check(ad.connect(), "connect on the fake bus")
+check(ad.rad_to_ticks("shoulder_pan", math.radians(30.0)) == 2389, f"+30 deg -> {ad.rad_to_ticks('shoulder_pan', math.radians(30.0))} ticks (2389)")
+check(abs(ad.ticks_to_rad("shoulder_lift", 860) * DEG + 104.4) < 0.05, f"860 ticks -> {ad.ticks_to_rad('shoulder_lift', 860) * DEG:.1f} deg (-104.4)")
+check(ad.gap_to_ticks(0.090) == 3674 and ad.gap_to_ticks(0.053) == 2780, "90 mm -> 3674, 53 mm -> 2780")
+check(abs(ad.ticks_to_gap(2780) - 0.053) < 1e-9 and abs(ad.ticks_to_gap(3674) - 0.090) < 1e-9, "and back")
+check(ad.gap_to_ticks(0.2) == 3674 and ad.gap_to_ticks(-1) == ad.gap_to_ticks(0.0), "the gap is clamped to 0..90 mm")
+check(ad.rad_to_ticks("shoulder_lift", -3.0) == 860 and ad.rad_to_ticks("shoulder_lift", 3.0) == 3236,
+ "arm goals are clamped to the calibration's end stops")
+
+# ── B. connect ──
+print("B. connect")
+w = bus.writes
+check(all((sid, F.REG_SPEED, 400) in w and (sid, F.REG_ACC, 50) in w for sid in range(1, 6)), "speed 400 and acceleration 50 on the five arm servos")
+check(all((6, r, 400) in w for r in F.GRIPPER_FORCE_REGS), "force 400 on the gripper's registers 44 and 48")
+check(not any(addr == F.REG_TORQUE for _, addr, _ in w), "no torque written at connect")
+check(ad.present_ticks() == {"shoulder_pan": 2857, "shoulder_lift": 860, "elbow_flex": 3206, "wrist_flex": 868, "wrist_roll": 3221, "gripper": 3674},
+ "the six present positions read")
+q = ad.read_joint_positions()
+check(abs(q[0] * DEG - 71.1) < 0.05 and abs(q[5] - 0.090) < 1e-9, f"positions: pan {q[0] * DEG:.1f} deg, gap {q[5] * 1000:.0f} mm")
+check(not ad.read_enabled() and ad.write_joint_positions(q) is False, "no command before activate")
+check(ad.activate() and ad.read_enabled() and all(s.torque == 1 for s in bus.servos.values()), "activate: torque on, six servos")
+check(ad.goal_ticks() == ad.present_ticks(), "torque on holds the present position")
+lim = ad.get_limits()
+check(abs(lim.position_lower[1] * DEG + 104.4) < 0.05 and abs(lim.position_upper[0] * DEG - 118.3) < 0.05 and lim.position_upper[5] == 0.090,
+ f"limits: lift from {lim.position_lower[1] * DEG:.1f} deg, pan to {lim.position_upper[0] * DEG:.1f} deg, gap to 90 mm")
+check(ad.has_live_state(), "live state after a read")
+
+# ── C. follow ──
+print("C. follow")
+target = list(q)
+target[0] = math.radians(30.0)
+n0 = len(bus.writes)
+check(ad.write_joint_positions(target), "a goal of +30 deg on the pan is written")
+check((1, F.REG_GOAL, 2389) in bus.writes[n0:] and len(bus.writes) - n0 == 1, "one write: servo 1, register 42, 2389")
+check(ad.write_joint_positions(target) and len(bus.writes) == n0 + 1, "the same goal again writes nothing")
+t0 = time.monotonic()
+while time.monotonic() - t0 < 2.0:
+ ad.read_joint_positions()
+ time.sleep(0.02)
+q = ad.read_joint_positions()
+check(abs(q[0] * DEG - 30.0) < 0.1, f"pan reads back {q[0] * DEG:+.2f} deg (2857 -> 2389 at 400 ticks/s took {abs(2857 - 2389) / 400:.1f} s)")
+target[5] = 0.053
+ad.write_joint_positions(target)
+check((6, F.REG_GOAL, 2780) in bus.writes, "the gripper to 53 mm writes 2780")
+time.sleep(0.5)
+q = ad.read_joint_positions()
+check(abs(q[5] - 0.053) < 0.0005, f"gap reads back {q[5] * 1000:.1f} mm")
+target[0] = math.radians(0.0)
+ad.write_joint_positions(target)
+time.sleep(0.3)
+ad.read_joint_positions()
+time.sleep(0.05)
+ad.read_joint_positions()
+v = ad.read_joint_velocities()
+check(v[0] < -math.radians(20), f"velocity from the reads: pan {v[0] * DEG:.0f} deg/s while moving (400 ticks/s = -35)")
+
+# ── D. watch ──
+print("D. watch")
+bus, ad = make(lag_ticks=150, lag_s=0.5, stale_s=0.3)
+ad.connect()
+ad.activate()
+bus.servos[2].block = 860 + 400 # the shoulder stops 338 ticks short of a goal 738 ticks up
+target = ad.read_joint_positions()
+target[1] = ad.ticks_to_rad("shoulder_lift", 860 + 738)
+ad.write_joint_positions(target)
+t0 = time.monotonic()
+code = 0
+while time.monotonic() - t0 < 3.0:
+ ad.read_joint_positions()
+ code, msg = ad.read_error()
+ if code:
+ break
+ time.sleep(0.02)
+el = time.monotonic() - t0
+check(code == F.FOLLOWING_ERROR and "shoulder_lift" in msg and "338" in msg,
+ f"FOLLOWING_ERROR after {el:.1f} s: {msg!r}")
+check(el > 1.0 + 0.5 - 0.2, f"not before the servo stood still (400 ticks at 400 ticks/s = 1.0 s, then lag_s 0.5 s: {el:.1f} s)")
+bus.servos[2].block = None
+t0 = time.monotonic()
+while time.monotonic() - t0 < 2.0 and ad.read_error()[0]:
+ ad.read_joint_positions()
+ time.sleep(0.02)
+check(ad.read_error()[0] == 0, "the error clears when the servo arrives")
+bus.dead = True
+q_last = ad.read_joint_positions()
+time.sleep(0.4)
+q_dead = ad.read_joint_positions()
+check(q_dead == q_last and not ad.has_live_state(), "a dead bus keeps the last positions and the state goes stale")
+
+# ── E. stop ──
+print("E. stop")
+bus, ad = make()
+ad.connect()
+ad.activate()
+t = ad.read_joint_positions()
+t[0] = math.radians(30.0)
+ad.write_joint_positions(t)
+time.sleep(0.3)
+ad.read_joint_positions()
+n0 = len(bus.writes)
+check(ad.write_stop(), "write_stop")
+goals = {sid: val for sid, addr, val in bus.writes[n0:] if addr == F.REG_GOAL}
+present = ad.present_ticks()
+check(goals[1] == present["shoulder_pan"] and len(goals) == 6, f"every goal set to the present position (pan {goals[1]}, mid-move)")
+check(ad.deactivate() and all(s.torque == 0 for s in bus.servos.values()) and not ad.read_enabled(), "deactivate: torque off")
+ad.disconnect()
+check(not bus.closed and not ad.is_connected(), "disconnect leaves the handed-in bus open, the adapter disconnected")
+
+# ── F. coordinator ──
+print("F. coordinator")
+try:
+ from dimos.control.coordinator import ControlCoordinator
+ from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionStatus
+ from dimos.hardware.manipulators.registry import adapter_registry
+except Exception as exc: # noqa: BLE001
+ print(f" (dimOS not importable here: {exc}; F skipped)")
+else:
+ try:
+ adapter_registry.register_path(A.REAL_ADAPTER, A.REAL_ADAPTER_PATH)
+ except ValueError:
+ pass
+ bus = FakeBus(DRIVE)
+ hw = A.arm_hardware(sim=False, address="/fake", calibration=CALIB, bus=bus)
+ coord = ControlCoordinator(publish_joint_state=False, tick_rate=50.0, hardware=[hw], tasks=[A.arm_trajectory_task()])
+ try:
+ coord.start()
+ time.sleep(0.3)
+ pos = coord.get_joint_positions()
+ check(abs(pos["arm/shoulder_pan"] * DEG - 71.1) < 0.1 and abs(pos["arm/gripper"] - 0.090) < 1e-9,
+ f"the coordinator reads the arm through the adapter: pan {pos['arm/shoulder_pan'] * DEG:.1f} deg, gap 90 mm")
+ check(all(s.torque == 1 for s in bus.servos.values()), "auto_enable: torque on at start")
+ drive = A.drive_pose_rad(CALIB)
+ q_pan = list(drive)
+ q_pan[0] = drive[0] + math.radians(30.0)
+ steps = [A.Step("line", "a", "b", list(drive), q_pan, 0.090, 0.090, 2.0, 2.0)]
+ traj = A.cycle_trajectory(steps, sim=False)
+ res = coord.execute_trajectory(traj)
+ check(res.status is TrajectoryExecutionStatus.ACCEPTED, f"trajectory accepted ({res.status.name})")
+ time.sleep(2.5)
+ pos = coord.get_joint_positions()
+ got = (pos["arm/shoulder_pan"] - drive[0]) * DEG
+ check(abs(got - 30.0) < 1.0, f"pan read back +{got:.2f} deg after a 2 s trajectory (goal writes on servo 1: {sum(1 for s, a, _ in bus.writes if s == 1 and a == F.REG_GOAL)})")
+ off = list(drive)
+ off[0] = drive[0] + math.radians(10.0)
+ bad = A.cycle_trajectory([A.Step("line", "a", "b", off, drive, 0.090, 0.090, 1.0, 1.0)], sim=False)
+ res = coord.execute_trajectory(bad)
+ check(res.status is TrajectoryExecutionStatus.START_STATE_MISMATCH, f"a trajectory 10 deg off the arm is refused ({res.status.name})")
+ finally:
+ coord.stop()
+ check(all(s.torque == 0 for s in bus.servos.values()), "stop: torque off")
+
+print(f"\n{OK} OK, {len(FAIL)} FAIL")
+for f in FAIL:
+ print(f" - {f}")
+sys.exit(1 if FAIL else 0)
diff --git a/benches/bench_moss_arm_sim_cold.py b/benches/bench_moss_arm_sim_cold.py
new file mode 100644
index 0000000..ac31419
--- /dev/null
+++ b/benches/bench_moss_arm_sim_cold.py
@@ -0,0 +1,313 @@
+"""Cold bench for moss_dimos.arm: the arm driven by dimOS in the MuJoCo
+simulation of the CAD scene, one process, no robot.
+
+Known input -> known output in physical units (degrees, millimetres,
+seconds). Groups:
+
+ A. units - ticks <-> model radians (2048 = 0, 2857 = +71.1 deg), the
+ simulator's gripper command and read-back (90 mm open ->
+ command 0 / slide 45 mm; 53 mm -> command 18.5 mm)
+ B. scene - the CAD scene is found; the simulation scene written from it
+ loads, its mesh directory is absolute, the arm joints' ranges
+ are the calibration's measured travel (shoulder_lift
+ +-104.4 deg where the delivered scene says +-100) and the
+ drive pose sits inside them, the three self-contact
+ exclusions are in, the twelve bodies are the CAD's; at the
+ drive pose the simulation scene has no contact where the
+ delivered scene has its folded-arm self-contacts
+ C. cycle - config/arm_drop_poses.json as delivered: ten moves, the
+ phases in order, as timed 36.9 s, stretched to 400 ticks/s
+ 54.6 s (the same numbers tools/moss_arm_drop.py plan prints);
+ the trajectory: six joints, t = 0 first, strictly increasing
+ times, 50 points per second, the gap 90 -> 53 mm over the
+ grasp and back over the release, every value finite
+ D. live - the sim module (headless) and the coordinator with the
+ sim_mujoco adapter and the trajectory task in this process:
+ the jaws start closed and stay so until a command;
+ (1) the pan +30 deg over 2 s reads +30 deg, then back, and
+ that first trajectory opens the jaws to 90 mm;
+ (2) the gripper commanded to 53 mm measures 53 mm between the
+ jaws in the model, 90 mm again when opened;
+ (3) the whole cycle at the ceiling, from the drive pose: the
+ arm ends at the drive pose, no arm-rover contact at any
+ sample, the following error stays small, the run takes the
+ stretched duration
+ E. resources - resident memory under 4 GB, the simulator's step cost
+
+Run (the dimOS main venv; MuJoCo and dimOS's sim module):
+ nice -n 10 <dimos venv>/bin/python benches/bench_moss_arm_sim_cold.py
+Env: MOSS_ARM_CAD (a MOSS_R31.xml or its directory) when the CAD is not at
+~/moss-*/cad/*/MOSS_R31.xml; MOSS_ARM_CALIB for a calibration file elsewhere.
+"""
+
+from __future__ import annotations
+
+import math
+import os
+import resource
+import sys
+import tempfile
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+if os.nice(0) < 10:
+ os.nice(10 - os.nice(0))
+
+from moss_dimos import arm as A # noqa: E402
+
+FAIL: list[str] = []
+OK = 0
+DEG = 180.0 / math.pi
+
+
+def check(cond: bool, what: str) -> None:
+ global OK
+ if cond:
+ OK += 1
+ print(f" ok {what}")
+ else:
+ FAIL.append(what)
+ print(f" FAIL {what}")
+
+
+def rss_gb() -> float:
+ return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1e6
+
+
+# ── A. units ──
+print("A. units")
+check(abs(A.ticks_to_rad(2048)) < 1e-12, "2048 ticks = 0 rad")
+check(abs(A.ticks_to_rad(2857) * DEG - 71.1) < 0.05, f"2857 ticks = +71.1 deg ({A.ticks_to_rad(2857) * DEG:.2f})")
+check(A.rad_to_ticks(A.ticks_to_rad(860)) == 860, "ticks -> rad -> ticks round trip at 860")
+check(abs(A.sim_gripper_command(0.090) - 0.0) < 1e-12, "90 mm open -> sim command 0")
+check(abs(A.sim_gripper_command(0.053) - 0.0185) < 1e-9, f"53 mm -> sim command 18.5 mm ({A.sim_gripper_command(0.053) * 1000:.2f})")
+check(abs(A.sim_gripper_command(0.0) - 0.045) < 1e-12, "0 mm closed -> sim command 45 mm")
+check(abs(A.sim_gripper_gap(0.0) - 0.090) < 1e-12 and abs(A.sim_gripper_gap(0.0185) - 0.053) < 1e-12,
+ "read back 0 -> gap 90 mm, 18.5 mm -> 53 mm (the read-back is in the command's convention)")
+check(all(abs(A.sim_gripper_gap(A.sim_gripper_command(g)) - g) < 1e-12 for g in (0.0, 0.03, 0.053, 0.09)),
+ "gap -> command -> read-back -> gap round trip")
+check(abs(A.velocity_limits()["arm/shoulder_pan"] - 400 / A.TICKS_PER_RAD) < 1e-12 and
+ abs(A.velocity_limits()["arm/shoulder_pan"] * DEG - 35.16) < 0.01,
+ "the task's ceiling on an arm joint = 400 ticks/s = 35.2 deg/s")
+
+# ── B. scene ──
+print("B. scene")
+cad = A.find_cad()
+check(cad is not None, f"CAD scene found: {cad}")
+if cad is None:
+ print("no CAD scene: B, D and E need it (MOSS_ARM_CAD)")
+ sys.exit(2)
+import mujoco # noqa: E402
+
+calib = os.environ.get("MOSS_ARM_CALIB") or None
+t0 = time.time()
+scene = A.sim_scene(cad, out_dir=Path(tempfile.mkdtemp(prefix="moss_arm_sim_")), calibration=calib)
+check(scene.is_file() and scene.name == A.SIM_SCENE_NAME, f"simulation scene written: {scene}")
+m_sim = mujoco.MjModel.from_xml_path(str(scene))
+load_s = time.time() - t0
+check(m_sim.nbody == 12, f"twelve bodies in the simulation scene (load + write {load_s:.1f} s)")
+import xml.etree.ElementTree as ET # noqa: E402
+
+root = ET.parse(scene).getroot()
+check(Path(root.find("compiler").get("meshdir")).is_absolute(), "mesh directory absolute")
+excl = {(e.get("body1"), e.get("body2")) for e in root.find("contact").findall("exclude")}
+check(all(p in excl for p in A.SIM_SELF_CONTACT_EXCLUDES), f"the three self-contact exclusions are in ({len(excl)} exclusions)")
+names = [mujoco.mj_id2name(m_sim, mujoco.mjtObj.mjOBJ_BODY, b) for b in range(m_sim.nbody)]
+check(all(b in names for b in (*A.ARM_BODIES, *A.ROVER_BODIES)), "arm and rover bodies named as expected")
+m_cad = mujoco.MjModel.from_xml_path(str(cad))
+drive = A.drive_pose_rad(calib)
+ranges = A.calibrated_ranges_rad(calib)
+check(bool(ranges), f"calibration found: {len(ranges)} joint travels" + ("" if ranges else " (set MOSS_ARM_CALIB)"))
+j_lift = mujoco.mj_name2id(m_sim, mujoco.mjtObj.mjOBJ_JOINT, "shoulder_lift")
+lo, hi = m_sim.jnt_range[j_lift] * DEG
+pad = A.SIM_RANGE_PAD_TICKS * 360.0 / A.TICKS_PER_REV
+check(abs(lo + 104.4 + pad) < 0.02 and abs(hi - 104.4 - pad) < 0.02,
+ f"simulation scene: shoulder_lift range {lo:+.2f}..{hi:+.2f} deg = the measured +-104.4 and a tick of slack (the delivered scene: +-100)")
+a_lift = mujoco.mj_name2id(m_sim, mujoco.mjtObj.mjOBJ_ACTUATOR, "shoulder_lift")
+check(abs(m_sim.actuator_ctrlrange[a_lift][0] - m_sim.jnt_range[j_lift][0]) < 1e-9, "its actuator's control range follows")
+a_jaw = mujoco.mj_name2id(m_sim, mujoco.mjtObj.mjOBJ_ACTUATOR, A.SIM_GRIPPER_ACTUATOR)
+check(abs(m_sim.actuator_gainprm[a_jaw][0] - A.SIM_GRIPPER_KP) < 1e-6 and abs(m_sim.actuator_forcerange[a_jaw][1] - A.SIM_GRIPPER_FORCE_N) < 1e-9,
+ f"the jaw actuator stiffened: kp {m_sim.actuator_gainprm[a_jaw][0]:g}, force +-{m_sim.actuator_forcerange[a_jaw][1]:g} N (delivered: 50, +-10)")
+check(all(m_sim.jnt_range[mujoco.mj_name2id(m_sim, mujoco.mjtObj.mjOBJ_JOINT, j)][0] <= q <= m_sim.jnt_range[mujoco.mj_name2id(m_sim, mujoco.mjtObj.mjOBJ_JOINT, j)][1]
+ for j, q in zip(A.ARM_JOINTS, drive)), "the drive pose sits inside the simulation scene's ranges")
+
+
+def contacts_at(model, q, slide=0.045):
+ d = mujoco.MjData(model)
+ d.qpos[:] = 0.0
+ d.qpos[:5] = q
+ d.qpos[5] = d.qpos[6] = slide
+ mujoco.mj_forward(model, d)
+ return [c for c in d.contact[: d.ncon] if c.dist < 0]
+
+
+raw = contacts_at(m_cad, drive)
+sim_c = contacts_at(m_sim, drive)
+check(len(raw) > 0 and len(sim_c) == 0,
+ f"drive pose: {len(raw)} penetrating self-contacts in the delivered scene, {len(sim_c)} in the simulation scene")
+
+# ── C. cycle ──
+print("C. cycle")
+cyc = A.load_cycle(calibration=calib)
+phases = [s.phase for s in cyc.steps]
+check(len(cyc.steps) == 10, f"ten moves ({len(cyc.steps)})")
+check(phases == ["leave_drive", "approach", "grasp", "lift", "clear", "transfer", "release", "back_to_clear", "hold", "to_drive"],
+ f"phases in order: {' '.join(phases)}")
+check(abs(cyc.timed_s - 36.9) < 0.05, f"as timed {cyc.timed_s:.1f} s")
+check(abs(cyc.duration_s - 54.6) < 0.1, f"stretched to 400 ticks/s {cyc.duration_s:.1f} s")
+check(cyc.steps[0].start == "drive" and cyc.steps[-1].end == "drive", "from drive, back to drive")
+check(all(abs(a - b) < 1e-12 for a, b in zip(cyc.first_pose, drive)), "the first pose is the drive pose")
+grasp = next(s for s in cyc.steps if s.phase == "grasp")
+check(abs(grasp.gap_from_m - 0.090) < 1e-9 and abs(grasp.gap_to_m - 0.053) < 1e-9 and grasp.duration_s == 1.0,
+ "grasp: 90 -> 53 mm in 1 s, the arm still")
+fastest = max(cyc.steps, key=lambda s: s.peak_ticks_per_s)
+check(abs(fastest.peak_ticks_per_s - 1810) < 5, f"the sequence's own peak {fastest.peak_ticks_per_s:.0f} ticks/s on {fastest.phase}")
+traj = A.cycle_trajectory(cyc.steps, sim=True)
+pts = traj.points
+check(traj.joint_names == A.JOINT_NAMES and all(len(p.positions) == 6 for p in pts), "six joints per point")
+check(pts[0].time_from_start == 0.0 and all(b.time_from_start > a.time_from_start for a, b in zip(pts[:-1], pts[1:])),
+ f"t = 0 first, strictly increasing ({len(pts)} points)")
+check(abs(traj.duration - cyc.duration_s) < 1e-6, f"trajectory duration {traj.duration:.2f} s")
+check(len(pts) / traj.duration > 45, f"{len(pts) / traj.duration:.0f} points per second")
+check(all(math.isfinite(v) for p in pts for v in (*p.positions, *p.velocities)), "every value finite")
+t_grasp_end = sum(s.duration_s for s in cyc.steps[:3])
+g_before = A.sim_gripper_gap(A.sample(traj, t_grasp_end - 1.0)[5])
+g_after = A.sim_gripper_gap(A.sample(traj, t_grasp_end)[5])
+check(abs(g_before - 0.090) < 1e-6 and abs(g_after - 0.053) < 1e-6,
+ f"gap in the trajectory: {g_before * 1000:.0f} mm before the grasp, {g_after * 1000:.0f} mm after")
+real = A.cycle_trajectory(cyc.steps, sim=False)
+check(abs(real.points[0].positions[5] - 0.090) < 1e-12 and abs(A.sample(real, t_grasp_end)[5] - 0.053) < 1e-6,
+ "on the robot the gripper rides as the gap in metres (0.090 then 0.053)")
+goto = A.goto_step([0.0] * 5, 0.090, drive, 0.090)
+check(goto.duration_s >= 1.0 and abs(goto.duration_s - A.QUINTIC_PEAK * max(abs(v) for v in drive) * A.TICKS_PER_RAD / 400) < 1e-9,
+ f"a goto from the zero pose to drive takes {goto.duration_s:.2f} s at the ceiling")
+
+# ── D. live ──
+print("D. live")
+from dimos.control.coordinator import ControlCoordinator # noqa: E402
+from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory # noqa: E402
+from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint # noqa: E402
+from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule # noqa: E402
+from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionStatus # noqa: E402
+
+sim_bp = A.sim_parts(scene, start_rad=drive)[0]
+sim_kwargs = dict(sim_bp.kwargs) if hasattr(sim_bp, "kwargs") else {}
+module = MujocoSimModule(address=str(scene), headless=True, dof=A.ARM_DOF, enable_color=False,
+ enable_depth=False, enable_pointcloud=False,
+ reset_joint_positions=[*drive, A.GAP_CLOSED_M])
+coord = None
+try:
+ t0 = time.time()
+ module.start()
+ engine = module._engine
+ check(module._gripper_idx == 5 and module._gripper_joint_range == (0.0, 0.045),
+ f"sim module up in {time.time() - t0:.1f} s: gripper = joint 5, range 0..45 mm")
+ hw = A.arm_hardware(sim=True, address=scene)
+ coord = ControlCoordinator(publish_joint_state=False, hardware=[hw], tasks=[A.arm_trajectory_task()])
+ coord.start()
+ time.sleep(1.5)
+ pos = coord.get_joint_positions()
+ check(set(pos) == set(A.JOINT_NAMES), f"coordinator joints: {sorted(pos)}")
+ at_drive = all(abs(pos[f"arm/{j}"] - drive[i]) * DEG < 0.5 for i, j in enumerate(A.ARM_JOINTS))
+ check(at_drive, "the simulated arm starts at the drive pose (within 0.5 deg)")
+ gap0 = A.sim_gripper_gap(pos["arm/gripper"])
+ check(abs(gap0 - 0.0) < 0.002, f"the jaws start closed and stay so until a command: {gap0 * 1000:.1f} mm (the adapter's read-back)")
+
+ def run(trajectory, label: str, poll_hz: float = 20.0):
+ """Execute and wait for the duration; return (max following error ticks,
+ arm-rover contacts seen, elapsed s, final positions)."""
+ res = coord.execute_trajectory(trajectory)
+ check(res.status is TrajectoryExecutionStatus.ACCEPTED, f"{label}: accepted ({res.status.name} {res.message})")
+ t_start = time.time()
+ worst = 0.0
+ contacts = 0
+ arm_ids = {names.index(b) for b in A.ARM_BODIES}
+ rover_ids = {names.index(b) for b in A.ROVER_BODIES}
+ while time.time() - t_start < trajectory.duration + 0.3:
+ t = time.time() - t_start
+ cur = coord.get_joint_positions()
+ err = A.following_error_ticks(trajectory, t, cur)
+ worst = max(worst, max(err.values(), default=0.0))
+ with engine._lock:
+ d = engine._data
+ for c in d.contact[: d.ncon]:
+ b1, b2 = m_sim.geom_bodyid[c.geom1], m_sim.geom_bodyid[c.geom2]
+ if c.dist < 0 and ((b1 in arm_ids and b2 in rover_ids) or (b2 in arm_ids and b1 in rover_ids)):
+ contacts += 1
+ time.sleep(1.0 / poll_hz)
+ return worst, contacts, time.time() - t_start, coord.get_joint_positions()
+
+ def line_trajectory(q0, q1, g0, g1, seconds):
+ steps = [A.Step("line", "a", "b", list(q0), list(q1), g0, g1, seconds, seconds)]
+ return A.cycle_trajectory(steps, sim=True)
+
+ def jaw_gap_mm() -> float:
+ with engine._lock:
+ d = engine._data
+ left, right = names.index("nc90_jaw_left"), names.index("nc90_jaw_right")
+ return float(((d.xpos[left] - d.xpos[right]) ** 2).sum() ** 0.5 * 1000.0)
+
+ # (1) pan +30 deg and back
+ q_pan = list(drive)
+ q_pan[0] = drive[0] + math.radians(30.0)
+ worst, contacts, el, fin = run(line_trajectory(drive, q_pan, 0.090, 0.090, 2.0), "pan +30 deg")
+ got = (fin["arm/shoulder_pan"] - drive[0]) * DEG
+ check(abs(got - 30.0) < 0.5, f"pan read back +{got:.2f} deg (commanded +30.00)")
+ check(abs(jaw_gap_mm() - 90.0) < 1.0, f"the first trajectory opened the jaws: {jaw_gap_mm():.1f} mm")
+ worst, contacts, el, fin = run(line_trajectory(q_pan, drive, 0.090, 0.090, 2.0), "pan back")
+ got = (fin["arm/shoulder_pan"] - drive[0]) * DEG
+ check(abs(got) < 0.5, f"pan back to {got:+.2f} deg")
+
+ # (2) the gripper: 53 mm then 90 mm, measured between the jaws in the model
+ run(line_trajectory(drive, drive, 0.090, 0.053, 1.0), "jaws to 53 mm")
+ time.sleep(1.5) # the jaws settle after the command (about half a second)
+ g53 = jaw_gap_mm()
+ check(abs(g53 - 53.0) < 1.0, f"jaw gap measured {g53:.1f} mm (commanded 53)")
+ g53_read = A.sim_gripper_gap(coord.get_joint_positions()["arm/gripper"]) * 1000
+ check(abs(g53_read - g53) < 1.0, f"the adapter's read-back says {g53_read:.1f} mm")
+ run(line_trajectory(drive, drive, 0.053, 0.090, 1.0), "jaws to 90 mm")
+ time.sleep(1.5)
+ g90 = jaw_gap_mm()
+ check(abs(g90 - 90.0) < 1.0, f"jaw gap measured {g90:.1f} mm (commanded 90)")
+
+ # (3) the whole cycle
+ step_ms = None
+ worst, contacts, el, fin = run(traj, "the cycle")
+ back = all(abs(fin[f"arm/{j}"] - drive[i]) * A.TICKS_PER_RAD < 20 for i, j in enumerate(A.ARM_JOINTS))
+ check(back, "the arm ends at the drive pose (within 20 ticks)")
+ check(contacts == 0, f"arm-rover contacts during the cycle: {contacts}")
+ check(worst < 40, f"worst following error {worst:.0f} ticks ({worst / A.TICKS_PER_RAD * DEG:.1f} deg)")
+ check(abs(el - (traj.duration + 0.3)) < 0.5, f"the cycle took {el:.1f} s (trajectory {traj.duration:.1f} s)")
+ time.sleep(1.5)
+ gap_end = jaw_gap_mm()
+ with engine._lock:
+ ctrl_end = float(engine._data.ctrl[5])
+ check(abs(gap_end - 90.0) < 1.0, f"jaws open at the end: {gap_end:.1f} mm (jaw ctrl {ctrl_end:.4f}, last commanded {traj.points[-1].positions[5]:.4f})")
+finally:
+ if coord is not None:
+ try:
+ coord.stop()
+ except Exception as exc: # noqa: BLE001
+ print(f" (coordinator stop: {exc})")
+ try:
+ module.stop()
+ except Exception as exc: # noqa: BLE001
+ print(f" (sim module stop: {exc})")
+
+# ── E. resources ──
+print("E. resources")
+d = mujoco.MjData(m_sim)
+t0 = time.time()
+for _ in range(200):
+ mujoco.mj_step(m_sim, d)
+step_ms = (time.time() - t0) / 200 * 1000
+check(step_ms < 5.0, f"simulator step {step_ms:.2f} ms (timestep {m_sim.opt.timestep * 1000:.0f} ms)")
+check(rss_gb() < 4.0, f"resident memory peak {rss_gb():.2f} GB")
+
+print(f"\n{OK} OK, {len(FAIL)} FAIL")
+for f in FAIL:
+ print(f" - {f}")
+sys.exit(1 if FAIL else 0)
diff --git a/benches/bench_moss_blueprints_cold.py b/benches/bench_moss_blueprints_cold.py
index 3094f8d..12a2187 100644
--- a/benches/bench_moss_blueprints_cold.py
+++ b/benches/bench_moss_blueprints_cold.py
@@ -1,4 +1,4 @@
-"""Cold bench for moss_dimos.blueprints: devices, the import, the four
+"""Cold bench for moss_dimos.blueprints: devices, the import, the five
blueprints, the coordinator in a fresh worker.
Known input -> known output in physical units. Groups:
@@ -21,8 +21,8 @@ Known input -> known output in physical units. Groups:
E. the camera - the base_link -> camera_link mount from the profile
(pitch -> quaternion)
H. wrist index - wrist_camera_index() on a fake /dev/v4l/by-id
- F. entry points - pyproject: distribution moss-dimos, the four entry points
- on the four lazy builders; then each one resolved through
+ F. entry points - pyproject: distribution moss-dimos, the five entry points
+ on the five lazy builders; then each one resolved through
dimOS's own `dimos run` lookup (get_by_name) from a
temporary dist-info built from pyproject.toml
G. blueprints - moss-base, moss-gamepad, moss-nav and moss-explore build:
@@ -243,7 +243,7 @@ check("the import runs in a clean interpreter", got is not None, f"rc {probe.ret
got = got or {}
check("'moss' unknown to the registry before the import", got.get("before") is None, f"{got.get('before')}")
check(f"after it: 'moss' -> {FACTORY} (the factory path is a string)", got.get("after") == FACTORY, f"{got.get('after')}")
-check("no blueprint built at import (all four are lazy)", got.get("built") == [], f"{got.get('built')}")
+check("no blueprint built at import (all five are lazy)", got.get("built") == [], f"{got.get('built')}")
check("no pad, lidar, map, planner, explorer, recorder or camera module imported with it",
got.get("heavy") == [], f"{got.get('heavy')}")
check("no environment variable set by the import (ROBOT_PROFILE, PERSISTENT_MAP untouched)",
@@ -374,9 +374,10 @@ eps = project.get("entry-points", {}).get("dimos.blueprints", {})
TARGETS = {"moss-base": "moss_dimos.blueprints:moss_base_blueprint",
"moss-gamepad": "moss_dimos.blueprints:moss_gamepad_blueprint",
"moss-nav": "moss_dimos.blueprints:moss_nav_blueprint",
- "moss-explore": "moss_dimos.blueprints:moss_explore_blueprint"}
+ "moss-explore": "moss_dimos.blueprints:moss_explore_blueprint",
+ "moss-arm": "moss_dimos.blueprints:moss_arm_blueprint"}
check("distribution name moss-dimos (the dimOS namespace)", project.get("name") == "moss-dimos", f"{project.get('name')}")
-check("exactly the four MOSS entry points, on moss_dimos.blueprints", eps == TARGETS, f"{eps}")
+check("exactly the five MOSS entry points, on moss_dimos.blueprints", eps == TARGETS, f"{eps}")
check("every target has a lazy builder", {t.split(":")[1] for t in eps.values()} == set(mb._BUILDERS),
f"{sorted(mb._BUILDERS)}")
check("names match dimOS's ^[a-z0-9]+(-[a-z0-9]+)*$", bool(eps) and all(re.match(r"^[a-z0-9]+(-[a-z0-9]+)*$", k) for k in eps))
@@ -398,8 +399,8 @@ from dimos.robot.external_blueprints import list_external_blueprint_names # noq
from dimos.robot.get_all_blueprints import get_by_name # noqa: E402
listed = [n for n in list_external_blueprint_names() if n.startswith("moss-dimos.")]
-check("dimOS lists moss-dimos.moss-base / moss-explore / moss-gamepad / moss-nav",
- listed == ["moss-dimos.moss-base", "moss-dimos.moss-explore", "moss-dimos.moss-gamepad", "moss-dimos.moss-nav"],
+check("dimOS lists moss-dimos.moss-arm / moss-base / moss-explore / moss-gamepad / moss-nav",
+ listed == ["moss-dimos.moss-arm", "moss-dimos.moss-base", "moss-dimos.moss-explore", "moss-dimos.moss-gamepad", "moss-dimos.moss-nav"],
f"{listed}")
resolved = {}
for local in TARGETS:
@@ -1083,9 +1084,9 @@ check("the registry builds the adapter from that string (as _setup_hardware does
w.get("adapter") == "moss_dimos.moss_adapter.MossBaseAdapter" and w.get("port") == "/dev/serial/by-id/MISSING-QT-PY",
f"{w.get('adapter')} on {w.get('port')}")
check("the LCM queue clamp is on in the worker", w.get("clamp") is True)
-check("the worker loads only the package, robot_profile, the adapter, the clamp and the wrapper",
- w.get("moss_modules") == ["moss_dimos", "moss_dimos.blueprints", "moss_dimos.latest_only", "moss_dimos.lcm_latest",
- "moss_dimos.moss_adapter", "moss_dimos.robot_profile"],
+check("the worker loads only the package, the arm's names (stdlib only), robot_profile, the adapter, the clamp and the wrapper",
+ w.get("moss_modules") == ["moss_dimos", "moss_dimos.arm", "moss_dimos.blueprints", "moss_dimos.latest_only",
+ "moss_dimos.lcm_latest", "moss_dimos.moss_adapter", "moss_dimos.robot_profile"],
f"{w.get('moss_modules')}")
WristCamera = mb.WristCamera
diff --git a/docs/arm.md b/docs/arm.md
index 52a1456..7a71359 100644
--- a/docs/arm.md
+++ b/docs/arm.md
@@ -457,3 +457,66 @@ python benches/bench_arm_drop_cold.py
grid step apart at most) with the guard, `segment_allowed` and the sweep's
box rule, prints one line per move with the reason of a refusal, and with
`--mujoco` counts the contacts of the whole sequence in the CAD scene.
+
+## The arm in dimOS
+
+`moss_dimos/arm.py` puts the arm under dimOS's control coordinator: one
+hardware component (`arm/shoulder_pan` … `arm/wrist_roll`, `arm/gripper`),
+one joint-trajectory task capped at the sweep's 400 ticks per second, and
+the cycle of `config/arm_drop_poses.json` sampled into one `JointTrajectory`
+(the quintic on the joints, the cubic on the jaw gap, every move stretched
+to the cap). Units are the model's radians and the jaw gap in metres; the
+adapters speak ticks.
+
+```sh
+dimos --simulation mujoco run moss-dimos.moss-arm # the CAD scene in MuJoCo, dimOS's sim adapter
+dimos run moss-dimos.moss-arm # the arm on its port (MOSS_ARM_PORT), the Feetech adapter
+tools/moss_arm_play.py [--sim] [--stop-after release] [--from back_to_clear] [--dry-run]
+```
+
+`moss-arm` is a coordinator alone; `tools/moss_arm_play.py` sends the cycle
+over RPC from wherever the arm is (a `goto` move first when it is not at
+the cycle's first pose), reads the joints back every 50 ms and prints the
+worst following error per joint; a joint more than `--max-lag` ticks behind
+for `--lag-seconds` cancels the trajectory and the run ends with `ARM LAG`.
+`--stop-after release` leaves the arm over the bin; `--from back_to_clear`
+is the return.
+
+**Simulation.** `arm.sim_scene` writes a simulation scene from the CAD
+scene into the MOSS state directory: the mesh directory made absolute, the
+arm joints' ranges and the actuators' control ranges set to the
+calibration's measured end stops (the delivered scene carries the upstream
+model's nominal ranges, which stop short of the drive pose: the lift 4,
+the elbow 5 and the wrist flex 9 degrees), the jaw actuator stiffened (the
+delivered one crawls), and the three self-contact pairs of the folded arm
+left out (in the delivered scene the forearm's collider sinks 16 mm into
+the shoulder's at the drive pose, where the real arm rests). Arm-rover
+contacts stay: `benches/bench_moss_arm_sim_cold.py` runs the whole cycle in
+this process and counts them (none), checks the joints read back what was
+commanded (the pan +30 degrees reads +30.00) and the jaws measure their
+gap (53 mm, 90 mm). The sim adapter's gripper value is inverted over the
+joint's range in both directions; `sim_gripper_command` and
+`sim_gripper_gap` are the only places that know it. The calibration file
+lives on the robot (`config/arm_calibration.json`); elsewhere point
+`MOSS_ARM_CALIB` at a copy.
+
+**The robot.** `moss_dimos/arm_feetech.py` is the `feetech_so101` adapter:
+scservo_sdk on the arm's port, one read of the six present positions per
+tick and a goal write for every servo whose target moved by a tick; goal
+speed 400 and acceleration 50 set once, the gripper's force limit 400 on
+registers 44 and 48, torque on at activate and off at deactivate. It
+watches what the coordinator does not: a servo more than 150 ticks from
+its goal and not moving for a second sets `FOLLOWING_ERROR` in
+`read_error()`; a bus that stops answering keeps the last positions and
+turns `has_live_state()` off, which stops the coordinator's commands.
+`benches/bench_moss_arm_feetech_cold.py` runs it on a fake bus, under the
+coordinator too. The coordinator ticks at 50 Hz on the robot: six reads
+and six writes at about 1.2 ms each fit in a tick.
+
+What the scene check found on the way: the keep-out table refuses the
+delivered travel and drop poses, but the CAD scene posed at one degree per
+joint between samples finds no contact along the whole sequence (1 021
+poses, 2 mm margin, the can held). The refusals are the table's grid
+(steps of 14 to 19 degrees, 49 on the roll: a forbidden neighbour 16 to
+31 degrees away), not the poses. The cycle therefore runs as delivered;
+the table stays the veto for moves the scene check has not seen.
diff --git a/moss_dimos/arm.py b/moss_dimos/arm.py
new file mode 100644
index 0000000..c49e292
--- /dev/null
+++ b/moss_dimos/arm.py
@@ -0,0 +1,456 @@
+"""The arm in dimOS: what the coordinator drives, what the simulator loads,
+and the pick-and-drop cycle as one joint trajectory.
+
+Three things live here, used by `moss_dimos.blueprints` (the `moss-arm`
+blueprint), `tools/moss_arm_play.py` (the executor) and the cold bench:
+
+* the hardware component: five joints and the gripper, under one adapter.
+ In simulation the adapter is dimOS's `sim_mujoco`, fed by a
+ `MujocoSimModule` loading the CAD scene (`sim_parts`); on the robot it is
+ the Feetech adapter of `moss_dimos.arm_feetech`. The coordinator's one
+ trajectory task runs the joints at the sweep's ceiling, no joint faster
+ than SPEED_TICKS_PER_S;
+* the simulation scene: the CAD scene with the joints' ranges set to the
+ arm's measured travel (the calibration's end stops; the delivered scene
+ carries the upstream model's nominal ranges, narrower than the drive pose)
+ and the arm's self-contacts of the folded pose left out (`sim_scene`),
+ written next to the other MOSS state;
+* the cycle of `config/arm_drop_poses.json` sampled into a
+ `JointTrajectory` (`cycle_trajectory`): each move a straight line in joint
+ space timed by the quintic, the jaw gap by the cubic, as the JSON says,
+ stretched where the sequence's own timing exceeds the ceiling.
+
+Units. Arm joints: the model's radians (the keep-out table's model); the
+gripper: the jaw gap in metres, 0 closed to GAP_OPEN_M. The simulator's
+gripper joint is the left jaw's slide in metres, half the gap, and dimOS's
+sim adapter inverts both the command and the read-back over the joint's
+range (a command of 0 drives the slide to its top, and reads back as 0):
+`sim_gripper_command` and `sim_gripper_gap` hold that, nothing else does. Ticks, for the messages people read, follow
+the keep-out table's rule: 2048 at the model's zero, 4096 per turn, sign +1.
+"""
+
+from __future__ import annotations
+
+import glob
+import json
+import math
+import os
+from dataclasses import dataclass, field
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+ROOT = Path(__file__).resolve().parent.parent
+POSES_PATH = ROOT / "config" / "arm_drop_poses.json"
+CALIBRATION_PATH = ROOT / "config" / "arm_calibration.json"
+CALIBRATION_ENV = "MOSS_ARM_CALIB" # a calibration file elsewhere (a rig without the robot's config)
+
+HW_ID = "arm"
+ARM_JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll")
+GRIPPER = "gripper"
+JOINT_NAMES = [f"{HW_ID}/{j}" for j in (*ARM_JOINTS, GRIPPER)] # the adapter's array order
+ARM_DOF = len(ARM_JOINTS)
+
+TICKS_PER_REV = 4096
+MODEL_ZERO_TICKS = 2048
+TICKS_PER_RAD = TICKS_PER_REV / (2.0 * math.pi)
+SPEED_TICKS_PER_S = 400.0 # the sweep's ceiling (docs/arm.md): no joint faster than this
+QUINTIC_PEAK = 1.875 # the quintic smootherstep's peak speed over its mean
+SAMPLE_HZ = 50.0 # trajectory points per second (the task interpolates between them)
+GAP_OPEN_M = 0.090 # the jaws fully open
+GAP_CLOSED_M = 0.0
+GRIPPER_SPEED_M_PER_S = 0.2 # the task's ceiling on the gripper: the whole gap in half a second
+START_TOLERANCE_RAD = 0.05 # the task refuses a trajectory starting further from the arm than this
+
+SIM_ADAPTER = "sim_mujoco" # dimOS's shared-memory MuJoCo adapter
+REAL_ADAPTER = "feetech_so101"
+REAL_ADAPTER_PATH = "moss_dimos.arm_feetech:FeetechSo101Adapter"
+ARM_PORT_ENV = "MOSS_ARM_PORT"
+ARM_PORT_DEFAULT = "/dev/serial/by-id/usb-1a86_USB_Single_Serial_5B3D041749-if00"
+
+CAD_ENV = "MOSS_ARM_CAD"
+CAD_GLOB = "~/moss-*/cad/*/MOSS_R31.xml"
+SIM_SCENE_NAME = "MOSS_R31_sim.xml"
+# The model's jaw slide (one jaw, metres) over the gap: both jaws move.
+SIM_SLIDE_PER_GAP = 0.5
+SIM_SLIDE_MAX_M = GAP_OPEN_M * SIM_SLIDE_PER_GAP
+# Pairs of arm links whose convex colliders overlap in the model when the arm
+# folds into the drive pose (the forearm back over the upper arm, 16 mm deep
+# shoulder-forearm in the delivered scene), where the real arm rests. Left out
+# of the simulation scene so the contact forces do not fight the folded pose;
+# arm-rover contacts stay, they are what the scene is for.
+SIM_SELF_CONTACT_EXCLUDES = (("shoulder", "lower_arm"), ("upper_arm", "wrist"), ("shoulder", "wrist"))
+# The measured end stops are integer ticks: a tick of slack each side keeps the
+# recorded poses on the stops (the drive pose) inside the simulated ranges.
+SIM_RANGE_PAD_TICKS = 1
+# The delivered scene's jaw actuator is soft (kp 50 N/m through the rack and
+# pinion: the jaws crawl at about a millimetre per second); the servo is stiff.
+# Set in the simulation scene so the jaws follow their command.
+SIM_GRIPPER_ACTUATOR = "nc90_close"
+SIM_GRIPPER_KP = 2000.0
+SIM_GRIPPER_FORCE_N = 20.0
+ARM_BODIES = ("shoulder", "upper_arm", "lower_arm", "wrist", "gripper",
+ "nc90_tool", "nc90_jaw_left", "nc90_jaw_right", "nc90_drive")
+ROVER_BODIES = ("rover_exterior", "base") # the arm's mount counts as rover
+
+# The pose the arm rests in for driving, as recorded on the arm (docs/arm.md);
+# config/arm_calibration.json carries it on the robot and wins when present.
+DRIVE_TICKS = {"shoulder_pan": 2857, "shoulder_lift": 860, "elbow_flex": 3206,
+ "wrist_flex": 868, "wrist_roll": 3221}
+
+
+# ── units ──
+
+def calibration_path(explicit: str | os.PathLike | None = None, env: dict | None = None) -> Path:
+ """The calibration file: `explicit`, else MOSS_ARM_CALIB, else config/arm_calibration.json."""
+ env = os.environ if env is None else env
+ raw = str(explicit or "").strip() or env.get(CALIBRATION_ENV, "").strip()
+ return Path(raw).expanduser() if raw else CALIBRATION_PATH
+
+
+def ticks_to_rad(ticks: float) -> float:
+ return (float(ticks) - MODEL_ZERO_TICKS) / TICKS_PER_RAD
+
+
+def rad_to_ticks(rad: float) -> int:
+ return int(round(MODEL_ZERO_TICKS + float(rad) * TICKS_PER_RAD))
+
+
+def sim_gripper_command(gap_m: float) -> float:
+ """The value to command on the simulator's gripper joint for a jaw gap:
+ the slide is half the gap, and the sim adapter inverts commands over the
+ joint's range."""
+ gap = min(max(float(gap_m), GAP_CLOSED_M), GAP_OPEN_M)
+ return SIM_SLIDE_MAX_M - gap * SIM_SLIDE_PER_GAP
+
+
+def sim_gripper_gap(value: float) -> float:
+ """The jaw gap for a value read back from the simulator's gripper joint:
+ the read-back is in the command's convention (inverted over the range),
+ so the slide is the top of the range less the value, and the gap twice
+ the slide."""
+ return (SIM_SLIDE_MAX_M - float(value)) / SIM_SLIDE_PER_GAP
+
+
+def quintic(u: float) -> float:
+ u = min(max(float(u), 0.0), 1.0)
+ return u * u * u * (u * (u * 6.0 - 15.0) + 10.0)
+
+
+def cubic(u: float) -> float:
+ u = min(max(float(u), 0.0), 1.0)
+ return u * u * (3.0 - 2.0 * u)
+
+
+# ── the CAD scene and the simulation scene ──
+
+def find_cad(spec: str | os.PathLike | None = None, env: dict | None = None) -> Path | None:
+ """The CAD scene: `spec` (a file, or a directory holding MOSS_R31.xml), else
+ MOSS_ARM_CAD, else the first match of CAD_GLOB; None when nothing exists."""
+ env = os.environ if env is None else env
+ raw = str(spec or "").strip() or env.get(CAD_ENV, "").strip()
+ candidates = [Path(raw).expanduser()] if raw else [Path(p) for p in sorted(glob.glob(os.path.expanduser(CAD_GLOB)))]
+ for c in candidates:
+ if c.is_dir():
+ c = c / "MOSS_R31.xml"
+ if c.is_file():
+ return c.resolve()
+ return None
+
+
+def state_dir(env: dict | None = None) -> Path:
+ """MOSS_STATE_DIR, default ~/.local/state/moss (the same rule as robot_profile)."""
+ env = os.environ if env is None else env
+ raw = env.get("MOSS_STATE_DIR", "").strip()
+ return Path(raw or "~/.local/state/moss").expanduser().absolute()
+
+
+def calibrated_ranges_rad(calibration: str | os.PathLike | None = None) -> dict[str, tuple[float, float]]:
+ """Each arm joint's measured travel in model radians, from the calibration's
+ end stops (reported ticks); empty when there is no calibration file."""
+ path = calibration_path(calibration)
+ if not path.is_file():
+ return {}
+ doc = json.loads(path.read_text())
+ out: dict[str, tuple[float, float]] = {}
+ for j in ARM_JOINTS:
+ rec = doc.get(j) or {}
+ if "range_min" in rec and "range_max" in rec:
+ out[j] = (ticks_to_rad(rec["range_min"] - SIM_RANGE_PAD_TICKS),
+ ticks_to_rad(rec["range_max"] + SIM_RANGE_PAD_TICKS))
+ return out
+
+
+def sim_scene(cad: str | os.PathLike, out_dir: str | os.PathLike | None = None,
+ calibration: str | os.PathLike | None = None) -> Path:
+ """The simulation scene written from the CAD scene: the mesh directory made
+ absolute (the file lives elsewhere), the arm joints' ranges and their
+ actuators' control ranges set to the measured travel when a calibration
+ exists (the delivered ranges stop short of the drive pose), the folded
+ pose's self-contacts excluded. Rewritten on every call; the path is the
+ SHM key the sim adapter and the sim module share, so both must get this
+ path."""
+ cad = Path(cad).resolve()
+ tree = ET.parse(cad)
+ root = tree.getroot()
+ ranges = calibrated_ranges_rad(calibration)
+ if ranges:
+ for el in root.iter("joint"):
+ r = ranges.get(el.get("name", ""))
+ if r:
+ el.set("range", f"{r[0]:.9f} {r[1]:.9f}")
+ act = root.find("actuator")
+ for el in (list(act) if act is not None else []):
+ r = ranges.get(el.get("name", "")) or ranges.get(el.get("joint", ""))
+ if r:
+ el.set("ctrlrange", f"{r[0]:.9f} {r[1]:.9f}")
+ if el.get("name") == SIM_GRIPPER_ACTUATOR:
+ el.set("kp", f"{SIM_GRIPPER_KP:g}")
+ el.set("forcerange", f"{-SIM_GRIPPER_FORCE_N:g} {SIM_GRIPPER_FORCE_N:g}")
+ compiler = root.find("compiler")
+ if compiler is None:
+ compiler = ET.SubElement(root, "compiler")
+ meshdir = Path(compiler.get("meshdir", "")) if compiler.get("meshdir") else Path(".")
+ if not meshdir.is_absolute():
+ meshdir = cad.parent / meshdir
+ compiler.set("meshdir", str(meshdir.resolve()))
+ contact = root.find("contact")
+ if contact is None:
+ contact = ET.SubElement(root, "contact")
+ have = {(e.get("body1"), e.get("body2")) for e in contact.findall("exclude")}
+ for a, b in SIM_SELF_CONTACT_EXCLUDES:
+ if (a, b) not in have and (b, a) not in have:
+ ET.SubElement(contact, "exclude", body1=a, body2=b)
+ out = Path(out_dir) if out_dir is not None else state_dir() / "arm_sim"
+ out.mkdir(parents=True, exist_ok=True)
+ path = out / SIM_SCENE_NAME
+ tree.write(path, encoding="utf-8", xml_declaration=True)
+ return path.resolve()
+
+
+# ── the hardware component, the task, the simulator ──
+
+def drive_pose_rad(calibration: str | os.PathLike | None = None) -> list[float]:
+ """The drive pose in model radians, in ARM_JOINTS order: from the
+ calibration file when it exists, else DRIVE_TICKS."""
+ path = calibration_path(calibration)
+ ticks = dict(DRIVE_TICKS)
+ if path.is_file():
+ doc = json.loads(path.read_text())
+ rec = (doc.get("poses") or {}).get("drive", {}).get("ticks") or {}
+ for j in ARM_JOINTS:
+ if j in rec:
+ ticks[j] = int(rec[j])
+ return [ticks_to_rad(ticks[j]) for j in ARM_JOINTS]
+
+
+def arm_port(env: dict | None = None) -> str:
+ env = os.environ if env is None else env
+ return env.get(ARM_PORT_ENV, "").strip() or ARM_PORT_DEFAULT
+
+
+def arm_hardware(sim: bool, address: str | os.PathLike | None = None,
+ calibration: str | os.PathLike | None = None, **adapter_kwargs):
+ """The one hardware component: in simulation the sim adapter on the
+ simulation scene (address = its path), on the robot the Feetech adapter on
+ the serial port."""
+ from dimos.control.components import HardwareComponent, HardwareType
+
+ if sim:
+ addr = str(address) if address else None
+ if addr is None:
+ cad = find_cad()
+ if cad is None:
+ raise FileNotFoundError(f"no CAD scene: {CAD_ENV}, or {CAD_GLOB}")
+ addr = str(sim_scene(cad, calibration=calibration))
+ return HardwareComponent(hardware_id=HW_ID, hardware_type=HardwareType.MANIPULATOR,
+ joints=list(JOINT_NAMES), adapter_type=SIM_ADAPTER, address=addr,
+ auto_enable=True, adapter_kwargs=dict(adapter_kwargs))
+ return HardwareComponent(hardware_id=HW_ID, hardware_type=HardwareType.MANIPULATOR,
+ joints=list(JOINT_NAMES), adapter_type=REAL_ADAPTER,
+ address=str(address) if address else arm_port(),
+ auto_enable=True, adapter_kwargs=dict(adapter_kwargs))
+
+
+def velocity_limits() -> dict[str, float]:
+ """The task's per-joint ceilings: the sweep's ticks per second in radians
+ per second on the arm, the gripper's in metres of gap per second."""
+ lim = {f"{HW_ID}/{j}": SPEED_TICKS_PER_S / TICKS_PER_RAD for j in ARM_JOINTS}
+ lim[f"{HW_ID}/{GRIPPER}"] = GRIPPER_SPEED_M_PER_S
+ return lim
+
+
+def arm_trajectory_task():
+ from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task
+
+ return joint_trajectory_task(list(JOINT_NAMES), start_position_tolerance=START_TOLERANCE_RAD,
+ velocity_limits=velocity_limits())
+
+
+def sim_parts(scene: str | os.PathLike, start_rad: list[float] | None = None, gap_m: float = GAP_CLOSED_M):
+ """The simulator module for the blueprint: headless, no camera, the arm
+ reset to `start_rad` (the drive pose by default). The jaws start at
+ `gap_m`, closed by default: the reset sets the left jaw's slide alone, and
+ the right jaw and the pinion (coupled to it) start at zero; the sim
+ adapter's first command opens them."""
+ from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule
+
+ start = list(start_rad if start_rad is not None else drive_pose_rad())
+ reset = [*start, gap_m * SIM_SLIDE_PER_GAP]
+ return (MujocoSimModule.blueprint(address=str(scene), headless=True, dof=ARM_DOF,
+ enable_color=False, enable_depth=False, enable_pointcloud=False,
+ reset_joint_positions=reset),)
+
+
+# ── the cycle as a trajectory ──
+
+@dataclass
+class Step:
+ phase: str
+ start: str
+ end: str
+ q_from: list[float] # ARM_JOINTS, model radians
+ q_to: list[float]
+ gap_from_m: float
+ gap_to_m: float
+ timed_s: float # the sequence's own duration
+ duration_s: float # as run: stretched to the speed ceiling
+ holds_can: bool = False
+
+ @property
+ def peak_ticks_per_s(self) -> float:
+ d = max(abs(b - a) for a, b in zip(self.q_from, self.q_to)) * TICKS_PER_RAD
+ return QUINTIC_PEAK * d / self.timed_s if self.timed_s > 0 else (math.inf if d else 0.0)
+
+
+@dataclass
+class Cycle:
+ steps: list[Step]
+ joints: list[str] = field(default_factory=lambda: list(ARM_JOINTS))
+
+ @property
+ def timed_s(self) -> float:
+ return sum(s.timed_s for s in self.steps)
+
+ @property
+ def duration_s(self) -> float:
+ return sum(s.duration_s for s in self.steps)
+
+ @property
+ def first_pose(self) -> list[float]:
+ return list(self.steps[0].q_from)
+
+ @property
+ def first_gap_m(self) -> float:
+ return self.steps[0].gap_from_m
+
+
+def stretched(q_a: list[float], q_b: list[float], timed_s: float, speed: float = SPEED_TICKS_PER_S) -> float:
+ d = max(abs(b - a) for a, b in zip(q_a, q_b)) * TICKS_PER_RAD
+ return max(timed_s, QUINTIC_PEAK * d / speed) if d else timed_s
+
+
+def load_cycle(path: str | os.PathLike = POSES_PATH, calibration: str | os.PathLike | None = None,
+ speed: float = SPEED_TICKS_PER_S, stop_after: str | None = None,
+ start: str | None = None) -> Cycle:
+ """The sequence of the JSON as delivered: its poses as they are (no shift,
+ no via), the drive pose from the calibration, each move stretched to
+ `speed`. `stop_after` keeps the moves up to that phase (e.g. "release":
+ the arm stays over the bin, the return is another run); `start` drops
+ the moves before that phase (e.g. "back_to_clear": the return)."""
+ doc = json.loads(Path(path).read_text())
+ joints = list(doc.get("joints") or ARM_JOINTS)
+ if tuple(joints) != ARM_JOINTS:
+ raise ValueError(f"{path}: joints {joints} are not {list(ARM_JOINTS)}")
+ poses = {name: [float(v) for v in rec["rad"]] for name, rec in doc["poses"].items()}
+ poses.setdefault("drive", drive_pose_rad(calibration))
+ steps: list[Step] = []
+ skipping = start is not None
+ for m in doc["sequence"]:
+ if skipping:
+ if m.get("phase") != start:
+ continue
+ skipping = False
+ a, b = m["from"], m["to"]
+ if a not in poses or b not in poses:
+ raise KeyError(f"{path}: move {m.get('phase')} uses an unknown pose ({a} -> {b})")
+ gap = m.get("gap_mm") or [GAP_OPEN_M * 1000, GAP_OPEN_M * 1000]
+ timed = float(m.get("duration_s", 0.0))
+ steps.append(Step(phase=str(m.get("phase", f"{a}_to_{b}")), start=a, end=b,
+ q_from=list(poses[a]), q_to=list(poses[b]),
+ gap_from_m=float(gap[0]) / 1000.0, gap_to_m=float(gap[1]) / 1000.0,
+ timed_s=timed, duration_s=stretched(poses[a], poses[b], timed, speed),
+ holds_can=bool(m.get("holds_can", False))))
+ if stop_after and steps[-1].phase == stop_after:
+ break
+ if not steps:
+ raise ValueError(f"{path}: empty sequence (start {start!r}, stop_after {stop_after!r})")
+ return Cycle(steps=steps, joints=joints)
+
+
+def goto_step(q_now: list[float], gap_now_m: float, q_to: list[float], gap_to_m: float,
+ speed: float = SPEED_TICKS_PER_S, least_s: float = 1.0) -> Step:
+ """A move from where the arm is to the cycle's first pose, at the ceiling."""
+ d = stretched(q_now, q_to, least_s, speed)
+ return Step(phase="goto", start="current", end="first", q_from=list(q_now), q_to=list(q_to),
+ gap_from_m=gap_now_m, gap_to_m=gap_to_m, timed_s=d, duration_s=d)
+
+
+def cycle_trajectory(steps: list[Step], sim: bool, sample_hz: float = SAMPLE_HZ):
+ """The steps sampled into one JointTrajectory in JOINT_NAMES order: arm
+ joints in radians, the gripper in the adapter's unit (the gap in metres on
+ the robot, the simulator's command in simulation). Each step runs the
+ quintic on the joints and the cubic on the gap over its duration; the
+ first point is at t = 0."""
+ from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory
+ from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint
+
+ g = sim_gripper_command if sim else (lambda gap: float(gap))
+ points: list[TrajectoryPoint] = []
+ t0 = 0.0
+ for s in steps:
+ n = max(1, int(math.ceil(s.duration_s * sample_hz)))
+ for k in range(0 if not points else 1, n + 1):
+ u = k / n
+ w = quintic(u)
+ q = [a + (b - a) * w for a, b in zip(s.q_from, s.q_to)]
+ gap = s.gap_from_m + (s.gap_to_m - s.gap_from_m) * cubic(u)
+ points.append(TrajectoryPoint(time_from_start=t0 + s.duration_s * u, positions=[*q, g(gap)]))
+ t0 += s.duration_s
+ # velocities: finite differences, for the record (the task interpolates positions)
+ for i, p in enumerate(points):
+ if i == 0 or i == len(points) - 1:
+ p.velocities = [0.0] * len(p.positions)
+ continue
+ a, b = points[i - 1], points[i + 1]
+ dt = b.time_from_start - a.time_from_start
+ p.velocities = [(y - x) / dt if dt > 0 else 0.0 for x, y in zip(a.positions, b.positions)]
+ return JointTrajectory(points=points, joint_names=list(JOINT_NAMES))
+
+
+def sample(trajectory, t: float) -> list[float]:
+ """The trajectory's positions at time t (linear between points, held at the ends)."""
+ pts = trajectory.points
+ if t <= pts[0].time_from_start:
+ return list(pts[0].positions)
+ for a, b in zip(pts[:-1], pts[1:]):
+ if a.time_from_start <= t <= b.time_from_start:
+ dt = b.time_from_start - a.time_from_start
+ w = (t - a.time_from_start) / dt if dt > 0 else 1.0
+ return [x + (y - x) * w for x, y in zip(a.positions, b.positions)]
+ return list(pts[-1].positions)
+
+
+def following_error_ticks(trajectory, t: float, measured: dict[str, float]) -> dict[str, float]:
+ """Per arm joint, |commanded - measured| in ticks at time t: the number
+ that says whether the arm follows (the shoulder short of its target under
+ load is the case this watches)."""
+ want = sample(trajectory, t)
+ out: dict[str, float] = {}
+ for name, w in zip(trajectory.joint_names, want):
+ if name == f"{HW_ID}/{GRIPPER}":
+ continue
+ m = measured.get(name)
+ if m is None:
+ continue
+ out[name] = abs(w - m) * TICKS_PER_RAD
+ return out
diff --git a/moss_dimos/arm_feetech.py b/moss_dimos/arm_feetech.py
new file mode 100644
index 0000000..107f114
--- /dev/null
+++ b/moss_dimos/arm_feetech.py
@@ -0,0 +1,419 @@
+"""The SO-101's Feetech servos as a dimOS manipulator adapter.
+
+`FeetechSo101Adapter` is what the coordinator creates for the `feetech_so101`
+hardware type (registered in `moss_dimos.blueprints`): six servos on one
+bus, the five arm joints in the model's radians and the gripper as the jaw
+gap in metres, the units `moss_dimos.arm` uses everywhere. Ticks come and go
+through the keep-out table's joint map (2048 at the model's zero, sign +1,
+`moss_dimos.arm_guard.JointMap`) and the calibration file (the servos'
+homing offsets and end stops); the gripper's ticks follow the straight
+line through the open and grasp values of config/arm_drop_poses.json.
+
+Per coordinator tick: one read of every servo's present position (register
+56), then one goal write (register 42) for every servo whose target moved
+by a tick or more. Goal speed (46) and acceleration (41) are set once at
+connect: the servo's own ceiling, so a streamed position cannot ask more
+than the sweep's speed. Torque (40) goes on at activate and off at
+deactivate; the gripper's force limit goes to registers 44 and 48 at
+connect.
+
+What the adapter watches, because the coordinator does not: the following
+error. Every read compares each arm servo's present position with its last
+goal; a servo more than `lag_ticks` from its goal and not moving (under
+`still_ticks_per_s`) for longer than `lag_s` sets the error code
+FOLLOWING_ERROR in `read_error()`, which the executor polls (the shoulder
+short of its target under load, stopped, is the case; a servo still on
+its way to a far goal is not).
+A read that fails leaves the last positions in place and, after
+`stale_s`, `has_live_state()` turns False so the coordinator stops
+commanding.
+
+The bus is scservo_sdk (PortHandler, PacketHandler protocol 0, 1 Mbps),
+wrapped in `FeetechBus` so a bench can hand in a fake with the same four
+calls. The SDK raises IndexError on a truncated reply: every call is
+retried up to three times.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import os
+import time
+from pathlib import Path
+from typing import Any
+
+from moss_dimos import arm as A
+from moss_dimos.arm_guard import JointMap, CALIBRATION_PATH as GUARD_CALIBRATION_PATH, TABLE_PATH
+
+REG_TORQUE, REG_ACC, REG_GOAL, REG_SPEED, REG_PRESENT = 40, 41, 42, 46, 56
+GRIPPER_FORCE_REGS = (44, 48)
+BAUD = 1_000_000
+PROTOCOL = 0
+SERVO_IDS = {"shoulder_pan": 1, "shoulder_lift": 2, "elbow_flex": 3, "wrist_flex": 4, "wrist_roll": 5, "gripper": 6}
+RETRIES = 3
+BUS_ERROR, FOLLOWING_ERROR = 1, 2
+DEFAULT_SPEED = int(A.SPEED_TICKS_PER_S) # register 46: steps per second
+DEFAULT_ACC = 50 # register 41: units of 100 steps per second squared
+DEFAULT_LAG_TICKS = 150 # 13 degrees
+DEFAULT_LAG_S = 1.0
+DEFAULT_STILL_TICKS_PER_S = 20.0 # below this the servo counts as stopped
+DEFAULT_STALE_S = 0.5
+GRIPPER_FORCE = 400
+# the gripper's line when config/arm_drop_poses.json is not there
+GRIPPER_LINE_DEFAULT = ((3674, 0.090), (2780, 0.053))
+
+
+def _signed(v: int) -> int:
+ """A present position as the servo sends it: bit 15 is the sign."""
+ return -(v & 0x7FFF) if v & 0x8000 else int(v)
+
+
+class FeetechBus:
+ """scservo_sdk behind four calls; opened lazily so a missing SDK fails at connect."""
+
+ def __init__(self, port: str, baud: int = BAUD) -> None:
+ self.port, self.baud = port, baud
+ self._scs = self._ph = self._pk = None
+
+ def open(self) -> bool:
+ import scservo_sdk as scs # noqa: PLC0415 - the robot's venv has it
+
+ ph = scs.PortHandler(self.port)
+ if not ph.openPort() or not ph.setBaudRate(self.baud):
+ return False
+ self._scs, self._ph, self._pk = scs, ph, scs.PacketHandler(PROTOCOL)
+ return True
+
+ def close(self) -> None:
+ if self._ph is not None:
+ try:
+ self._ph.closePort()
+ except Exception: # noqa: BLE001
+ pass
+ self._scs = self._ph = self._pk = None
+
+ def read2(self, sid: int, addr: int) -> int | None:
+ for _ in range(RETRIES):
+ try:
+ v, comm, _err = self._pk.read2ByteTxRx(self._ph, sid, addr)
+ if comm == self._scs.COMM_SUCCESS:
+ return int(v)
+ except Exception: # noqa: BLE001 - a short packet inside the SDK
+ pass
+ return None
+
+ def _write(self, fn, sid: int, addr: int, value: int) -> bool:
+ for _ in range(RETRIES):
+ try:
+ comm, _err = fn(self._ph, sid, addr, int(value))
+ if comm == self._scs.COMM_SUCCESS:
+ return True
+ except Exception: # noqa: BLE001
+ pass
+ return False
+
+ def write1(self, sid: int, addr: int, value: int) -> bool:
+ return self._write(self._pk.write1ByteTxRx, sid, addr, value)
+
+ def write2(self, sid: int, addr: int, value: int) -> bool:
+ return self._write(self._pk.write2ByteTxRx, sid, addr, value)
+
+
+def gripper_line(poses_path: str | os.PathLike | None = None) -> tuple[tuple[int, float], tuple[int, float]]:
+ """(ticks, gap m) at open and at grasp, from the cycle's JSON when present."""
+ path = Path(poses_path) if poses_path else A.POSES_PATH
+ if path.is_file():
+ g = json.loads(path.read_text()).get("gripper") or {}
+ if "open" in g and "grasp" in g:
+ return ((int(g["open"]["ticks"]), float(g["open"]["gap_mm"]) / 1000.0),
+ (int(g["grasp"]["ticks"]), float(g["grasp"]["gap_mm"]) / 1000.0))
+ return GRIPPER_LINE_DEFAULT
+
+
+class FeetechSo101Adapter:
+ """`ManipulatorAdapter` for the SO-101 on Feetech STS3215 servos (and the
+ gripper's servo), units of moss_dimos.arm."""
+
+ def __init__(self, dof: int = 6, address: str | None = None, hardware_id: str | None = None,
+ *, calibration: str | None = None, table: str | None = None, poses: str | None = None,
+ ids: dict | None = None, speed: int = DEFAULT_SPEED, acc: int = DEFAULT_ACC,
+ lag_ticks: float = DEFAULT_LAG_TICKS, lag_s: float = DEFAULT_LAG_S, stale_s: float = DEFAULT_STALE_S,
+ still_ticks_per_s: float = DEFAULT_STILL_TICKS_PER_S,
+ gripper_force: int = GRIPPER_FORCE, bus: Any | None = None, **_: Any) -> None:
+ if dof != len(A.JOINT_NAMES):
+ raise ValueError(f"feetech_so101 drives {len(A.JOINT_NAMES)} joints, not {dof}")
+ self._dof = dof
+ self._hardware_id = hardware_id
+ self._port = str(address) if address else A.arm_port()
+ self._calibration = Path(calibration) if calibration else GUARD_CALIBRATION_PATH
+ self._table = Path(table) if table else TABLE_PATH
+ self._ids = dict(ids or SERVO_IDS)
+ self._speed, self._acc = int(speed), int(acc)
+ self._lag_ticks, self._lag_s, self._stale_s = float(lag_ticks), float(lag_s), float(stale_s)
+ self._still = float(still_ticks_per_s)
+ self._tick_vel: dict[str, float] = {} # ticks per second, from the last two reads
+ self._prev_read: tuple[float, dict[str, int]] | None = None
+ self._gripper_force = int(gripper_force)
+ self._bus = bus
+ self._owns_bus = bus is None
+ self._map: JointMap | None = None
+ self._ranges: dict[str, tuple[int, int]] = {}
+ (self._g_open_ticks, self._g_open_m), (self._g_grasp_ticks, self._g_grasp_m) = gripper_line(poses)
+ self._ticks_per_m = (self._g_open_ticks - self._g_grasp_ticks) / (self._g_open_m - self._g_grasp_m)
+ self._connected = False
+ self._enabled = False
+ self._present: dict[str, int] = {} # the last good read, ticks
+ self._goal: dict[str, int] = {} # the last goal written, ticks
+ self._last_good = 0.0
+ self._lag_since: dict[str, float] = {}
+ self._error = (0, "")
+ self._prev: tuple[float, list[float]] | None = None
+ self._velocities = [0.0] * dof
+ self._control_mode = None
+
+ # ── units ──
+
+ def _joints(self) -> list[str]:
+ return [*A.ARM_JOINTS, A.GRIPPER]
+
+ def gap_to_ticks(self, gap_m: float) -> int:
+ gap = min(max(float(gap_m), A.GAP_CLOSED_M), A.GAP_OPEN_M)
+ return int(round(self._g_open_ticks + (gap - self._g_open_m) * self._ticks_per_m))
+
+ def ticks_to_gap(self, ticks: float) -> float:
+ return self._g_open_m + (float(ticks) - self._g_open_ticks) / self._ticks_per_m
+
+ def rad_to_ticks(self, joint: str, rad: float) -> int:
+ t = self._map.rad_to_ticks(joint, rad) if self._map else A.rad_to_ticks(rad)
+ lo, hi = self._ranges.get(joint, (0, A.TICKS_PER_REV - 1))
+ return int(min(max(t, lo), hi))
+
+ def ticks_to_rad(self, joint: str, ticks: float) -> float:
+ return self._map.ticks_to_rad(joint, ticks) if self._map else A.ticks_to_rad(ticks)
+
+ # ── lifecycle ──
+
+ def connect(self) -> bool:
+ if self._bus is None:
+ self._bus = FeetechBus(self._port)
+ if self._owns_bus and not self._bus.open():
+ self._error = (BUS_ERROR, f"cannot open {self._port}")
+ return False
+ try:
+ self._map = JointMap.load(self._calibration, self._table)
+ except Exception as exc: # noqa: BLE001 - no table or calibration: the plain rule
+ self._map = None
+ self._error = (0, f"joint map not loaded ({exc}); the plain 2048 rule is used")
+ if self._calibration.is_file():
+ doc = json.loads(self._calibration.read_text())
+ for j in A.ARM_JOINTS:
+ rec = doc.get(j) or {}
+ if "range_min" in rec and "range_max" in rec:
+ self._ranges[j] = (int(rec["range_min"]), int(rec["range_max"]))
+ g = doc.get("gripper") or {}
+ if "range_min" in g and "range_max" in g:
+ self._ranges[A.GRIPPER] = (int(g["range_min"]), int(g["range_max"]))
+ if self._read_all() is None:
+ self._error = (BUS_ERROR, "the servos do not answer")
+ return False
+ ok = True
+ for j in A.ARM_JOINTS:
+ sid = self._ids[j]
+ ok &= self._bus.write1(sid, REG_ACC, self._acc)
+ ok &= self._bus.write2(sid, REG_SPEED, self._speed)
+ gid = self._ids[A.GRIPPER]
+ for reg in GRIPPER_FORCE_REGS:
+ ok &= self._bus.write2(gid, reg, self._gripper_force)
+ if not ok:
+ self._error = (BUS_ERROR, "a servo did not take its speed, acceleration or force setting")
+ return False
+ self._goal = dict(self._present)
+ self._connected = True
+ return True
+
+ def disconnect(self) -> None:
+ if self._connected and self._enabled:
+ try:
+ self.write_enable(False)
+ except Exception: # noqa: BLE001
+ pass
+ if self._owns_bus and self._bus is not None:
+ self._bus.close()
+ self._connected = False
+
+ def is_connected(self) -> bool:
+ return self._connected
+
+ def has_live_state(self) -> bool:
+ return self._connected and (time.monotonic() - self._last_good) < self._stale_s
+
+ def activate(self) -> bool:
+ return self.write_enable(True)
+
+ def deactivate(self) -> bool:
+ self.write_stop()
+ return self.write_enable(False)
+
+ def get_info(self):
+ from dimos.hardware.manipulators.spec import ManipulatorInfo
+
+ return ManipulatorInfo(vendor="Feetech", model="SO-101 STS3215", dof=self._dof,
+ firmware_version=None, serial_number=None)
+
+ def get_dof(self) -> int:
+ return self._dof
+
+ def get_limits(self):
+ from dimos.hardware.spec import JointLimits
+
+ lo, hi = [], []
+ for j in A.ARM_JOINTS:
+ r = self._ranges.get(j)
+ lo.append(self.ticks_to_rad(j, r[0]) if r else -math.pi)
+ hi.append(self.ticks_to_rad(j, r[1]) if r else math.pi)
+ return JointLimits(position_lower=[*lo, A.GAP_CLOSED_M], position_upper=[*hi, A.GAP_OPEN_M],
+ velocity_max=[self._speed / A.TICKS_PER_RAD] * A.ARM_DOF + [A.GRIPPER_SPEED_M_PER_S])
+
+ def set_control_mode(self, mode) -> bool:
+ from dimos.hardware.manipulators.spec import ControlMode
+
+ self._control_mode = mode
+ return mode in (ControlMode.POSITION, ControlMode.SERVO_POSITION)
+
+ def get_control_mode(self):
+ from dimos.hardware.manipulators.spec import ControlMode
+
+ return self._control_mode or ControlMode.POSITION
+
+ # ── reads ──
+
+ def _read_all(self) -> dict[str, int] | None:
+ out: dict[str, int] = {}
+ for j in self._joints():
+ v = self._bus.read2(self._ids[j], REG_PRESENT)
+ if v is None:
+ return None
+ out[j] = _signed(v)
+ now = time.monotonic()
+ if self._prev_read is not None and now > self._prev_read[0]:
+ dt = now - self._prev_read[0]
+ self._tick_vel = {j: (out[j] - self._prev_read[1].get(j, out[j])) / dt for j in out}
+ self._prev_read = (now, dict(out))
+ self._present = out
+ self._last_good = now
+ self._watch_lag(now)
+ return out
+
+ def _watch_lag(self, now: float) -> None:
+ if not self._enabled:
+ self._lag_since.clear()
+ return
+ lagging = None
+ for j in A.ARM_JOINTS:
+ g = self._goal.get(j)
+ p = self._present.get(j)
+ if g is None or p is None:
+ continue
+ if abs(g - p) > self._lag_ticks and abs(self._tick_vel.get(j, 0.0)) < self._still:
+ self._lag_since.setdefault(j, now)
+ if now - self._lag_since[j] >= self._lag_s and lagging is None:
+ lagging = (j, g - p, now - self._lag_since[j])
+ else:
+ self._lag_since.pop(j, None)
+ if lagging is not None:
+ j, d, s = lagging
+ self._error = (FOLLOWING_ERROR, f"{j} is {abs(d):.0f} ticks {'short of' if d > 0 else 'past'} its goal for {s:.1f} s")
+ elif self._error[0] == FOLLOWING_ERROR:
+ self._error = (0, "")
+
+ def read_joint_positions(self) -> list[float]:
+ got = self._read_all()
+ src = got if got is not None else self._present
+ if not src:
+ return [0.0] * self._dof
+ q = [self.ticks_to_rad(j, src[j]) for j in A.ARM_JOINTS] + [self.ticks_to_gap(src[A.GRIPPER])]
+ now = time.monotonic()
+ if got is not None and self._prev is not None and now > self._prev[0]:
+ dt = now - self._prev[0]
+ self._velocities = [(b - a) / dt for a, b in zip(self._prev[1], q)]
+ if got is not None:
+ self._prev = (now, q)
+ return q
+
+ def read_joint_velocities(self) -> list[float]:
+ return list(self._velocities)
+
+ def read_joint_efforts(self) -> list[float]:
+ return [0.0] * self._dof
+
+ def read_state(self) -> dict[str, int]:
+ moving = any(abs(v) > 1e-3 for v in self._velocities)
+ return {"state": 1 if moving else 0, "mode": 0}
+
+ def read_error(self) -> tuple[int, str]:
+ return self._error
+
+ def present_ticks(self) -> dict[str, int]:
+ return dict(self._present)
+
+ def goal_ticks(self) -> dict[str, int]:
+ return dict(self._goal)
+
+ # ── writes ──
+
+ def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool:
+ if not self._connected or not self._enabled or len(positions) != self._dof:
+ return False
+ want = {j: self.rad_to_ticks(j, positions[i]) for i, j in enumerate(A.ARM_JOINTS)}
+ want[A.GRIPPER] = self.gap_to_ticks(positions[A.ARM_DOF])
+ ok = True
+ for j, t in want.items():
+ if self._goal.get(j) == t:
+ continue
+ if self._bus.write2(self._ids[j], REG_GOAL, t):
+ self._goal[j] = t
+ else:
+ ok = False
+ if not ok:
+ self._error = (BUS_ERROR, "a goal write failed")
+ return ok
+
+ def write_joint_velocities(self, velocities: list[float]) -> bool:
+ return False
+
+ def write_joint_efforts(self, efforts: list[float]) -> bool:
+ return False
+
+ def write_stop(self) -> bool:
+ if not self._connected or not self._present:
+ return False
+ ok = True
+ for j in self._joints():
+ t = self._present[j]
+ if self._bus.write2(self._ids[j], REG_GOAL, t):
+ self._goal[j] = t
+ else:
+ ok = False
+ return ok
+
+ def write_enable(self, enable: bool) -> bool:
+ if not self._connected:
+ return False
+ ok = True
+ for j in self._joints():
+ ok &= self._bus.write1(self._ids[j], REG_TORQUE, 1 if enable else 0)
+ if ok:
+ self._enabled = bool(enable)
+ if enable:
+ self._goal = dict(self._present) # torque on: hold where the arm is
+ self._lag_since.clear()
+ return ok
+
+ def read_enabled(self) -> bool:
+ return self._enabled
+
+ def write_clear_errors(self) -> bool:
+ self._error = (0, "")
+ self._lag_since.clear()
+ return True
diff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.py
index 810131f..d928615 100644
--- a/moss_dimos/blueprints.py
+++ b/moss_dimos/blueprints.py
@@ -150,6 +150,17 @@ except ValueError as _exc:
f"{_existing or 'another path'!r}; {MOSS_ADAPTER_PATH!r} is NOT used "
f"in this process ({_exc})")
+# The arm's adapter, the same way (moss_dimos.arm_feetech; docs/arm.md).
+from dimos.hardware.manipulators.registry import adapter_registry as _manipulator_registry # noqa: E402
+from moss_dimos.arm import REAL_ADAPTER as ARM_ADAPTER_NAME, REAL_ADAPTER_PATH as ARM_ADAPTER_PATH # noqa: E402
+try:
+ _manipulator_registry.register_path(ARM_ADAPTER_NAME, ARM_ADAPTER_PATH)
+except ValueError as _exc:
+ _existing = getattr(_manipulator_registry, "_factory_paths", {}).get(ARM_ADAPTER_NAME)
+ _logger.warning(f"manipulator adapter '{ARM_ADAPTER_NAME}' is already registered as "
+ f"{_existing or 'another path'!r}; {ARM_ADAPTER_PATH!r} is NOT used "
+ f"in this process ({_exc})")
+
# Velocity commands must never queue up (stale twists drive the rover past
# the point asked): clamp the LCM queue of command channels to one message.
from moss_dimos.lcm_latest import install as _install_latest_only # noqa: E402
@@ -638,7 +649,7 @@ def _jpeg_camera(bp):
return bp.transports(transports) if transports else bp
-# ── the four blueprints, built on first access ──────────────────────────────
+# ── the blueprints, built on first access ──────────────────────────────
def build_moss_base():
"""The coordinator and the tracked base, twist_command on cmd_vel: any
@@ -716,8 +727,45 @@ def build_moss_explore():
])
+def build_moss_arm():
+ """The arm alone: one coordinator with the six-joint component and the
+ trajectory task (tools/moss_arm_play.py sends the cycle over RPC).
+ `dimos --simulation mujoco run moss-dimos.moss-arm` adds the simulator on
+ the CAD scene and drives the sim adapter; without the flag the Feetech
+ adapter opens the arm's port (MOSS_ARM_PORT). Ticks at 50 Hz on the
+ robot: six reads and up to six writes on a 1 Mbps bus fit in a tick."""
+ from dimos.control.coordinator import ControlCoordinator
+ from dimos.core.coordination.blueprints import autoconnect
+ from dimos.core.global_config import global_config
+ from moss_dimos import arm
+
+ sim = bool(global_config.simulation)
+ if sim:
+ cad = arm.find_cad()
+ if cad is None:
+ raise FileNotFoundError(f"moss-arm in simulation needs the CAD scene: {arm.CAD_ENV}, or {arm.CAD_GLOB}")
+ scene = arm.sim_scene(cad)
+ _logger.info(f"moss-arm: simulation scene {scene} from {cad}")
+ hw = arm.arm_hardware(sim=True, address=scene)
+ extra = arm.sim_parts(scene)
+ tick = 100.0
+ else:
+ hw = arm.arm_hardware(sim=False)
+ extra = ()
+ tick = ARM_TICK_HZ
+ return autoconnect(
+ ControlCoordinator.blueprint(hardware=[hw], tasks=[arm.arm_trajectory_task()],
+ tick_rate=tick, instance_name=COORDINATOR_INSTANCE_NAME),
+ *extra,
+ )
+
+
+ARM_TICK_HZ = 50.0 # the real bus: six reads and six writes at about 1.2 ms each
+
+
# pyproject entry point target -> builder. Keys must equal the targets.
_BUILDERS = {"moss_base_blueprint": build_moss_base,
+ "moss_arm_blueprint": build_moss_arm,
"moss_gamepad_blueprint": build_moss_gamepad,
"moss_nav_blueprint": build_moss_nav,
"moss_explore_blueprint": build_moss_explore}
diff --git a/pyproject.toml b/pyproject.toml
index dcad08c..c120f6c 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -33,6 +33,7 @@ moss-base = "moss_dimos.blueprints:moss_base_blueprint"
moss-gamepad = "moss_dimos.blueprints:moss_gamepad_blueprint"
moss-nav = "moss_dimos.blueprints:moss_nav_blueprint"
moss-explore = "moss_dimos.blueprints:moss_explore_blueprint"
+moss-arm = "moss_dimos.blueprints:moss_arm_blueprint"
[tool.setuptools]
packages = ["moss_dimos", "moss_pi"]
diff --git a/tools/moss_arm_play.py b/tools/moss_arm_play.py
new file mode 100644
index 0000000..cb5f2f3
--- /dev/null
+++ b/tools/moss_arm_play.py
@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+"""Play the pick-and-drop cycle on the arm through the running `moss-arm`
+coordinator (simulation or robot), watching the arm follow.
+
+ dimos --simulation mujoco run moss-dimos.moss-arm # one terminal: the simulator and the coordinator
+ tools/moss_arm_play.py --sim # another: the cycle, from wherever the arm is
+ tools/moss_arm_play.py --sim --stop-after release # up to the release: the arm waits over the bin
+ tools/moss_arm_play.py --sim --dry-run # the trajectory's numbers, nothing sent
+
+On the robot the same, without --sim, after `dimos run moss-dimos.moss-arm`
+(the Feetech adapter on the arm's port). Nothing here checks the scene:
+the cycle is the one tools/moss_arm_drop.py plan --mujoco verified in the CAD
+scene, run as delivered. What this tool adds is the watch: every 50 ms it
+reads the joints back and compares them with the trajectory; a joint more
+than --max-lag ticks behind for longer than --lag-seconds cancels the
+trajectory (the coordinator holds the last commanded position) and the run
+ends with ARM LAG and the joint's numbers. That is the case of the shoulder
+short of its target under load; the number is printed on every run, lag or
+not, so the gap between the simulation and the arm is a measurement.
+
+Options:
+ --sim the gripper in the simulator's unit (see moss_dimos.arm)
+ --poses PATH the cycle (config/arm_drop_poses.json)
+ --calib PATH the calibration (the drive pose); MOSS_ARM_CALIB, else config/arm_calibration.json
+ --speed TICKS_PER_S the ceiling (400)
+ --stop-after PHASE end the cycle after this phase (e.g. release)
+ --from PHASE start the cycle at this phase (e.g. back_to_clear: the return from the bin)
+ --max-lag TICKS the following error that counts as a lag (150 = 13 deg)
+ --lag-seconds S how long a lag must last before the stop (1.0)
+ --no-goto refuse to start unless the arm is at the cycle's first pose
+ --dry-run print the steps and the trajectory, send nothing
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+
+from moss_dimos import arm as A # noqa: E402
+
+POLL_S = 0.05
+DEG = 180.0 / math.pi
+
+
+def say(*a):
+ print(*a, flush=True)
+
+
+def main() -> int:
+ ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
+ ap.add_argument("--sim", action="store_true")
+ ap.add_argument("--poses", default=str(A.POSES_PATH))
+ ap.add_argument("--calib", default=None)
+ ap.add_argument("--speed", type=float, default=A.SPEED_TICKS_PER_S)
+ ap.add_argument("--stop-after", default=None)
+ ap.add_argument("--from", dest="start", default=None)
+ ap.add_argument("--max-lag", type=float, default=150.0)
+ ap.add_argument("--lag-seconds", type=float, default=1.0)
+ ap.add_argument("--no-goto", action="store_true")
+ ap.add_argument("--dry-run", action="store_true")
+ a = ap.parse_args()
+
+ cycle = A.load_cycle(a.poses, calibration=a.calib, speed=a.speed, stop_after=a.stop_after, start=a.start)
+ say(f"cycle: {len(cycle.steps)} moves, {cycle.timed_s:.1f} s as timed, {cycle.duration_s:.1f} s at {a.speed:g} ticks/s")
+ for s in cycle.steps:
+ say(f" {s.phase:14s} {s.start:>7s} -> {s.end:<7s} {s.duration_s:5.2f} s gap {s.gap_from_m * 1000:.0f} -> {s.gap_to_m * 1000:.0f} mm"
+ f"{' (holds the can)' if s.holds_can else ''}")
+ if a.dry_run:
+ traj = A.cycle_trajectory(cycle.steps, sim=a.sim)
+ say(f"trajectory: {len(traj.points)} points, {traj.duration:.2f} s, joints {traj.joint_names}")
+ for p in traj.points[:: max(1, len(traj.points) // 12)]:
+ say(f" t {p.time_from_start:6.2f} " + " ".join(f"{A.rad_to_ticks(v):5d}" for v in p.positions[:5]) + f" gripper {p.positions[5]:.4f}")
+ return 0
+
+ from dimos.control.coordinator import ControlCoordinator
+ from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionStatus
+ from dimos.core.rpc_client import RPCClient
+
+ client = RPCClient(None, ControlCoordinator)
+ try:
+ now = client.get_joint_positions() or {}
+ missing = [j for j in A.JOINT_NAMES if j not in now]
+ if missing:
+ say(f"refused: the coordinator does not report {missing} (is moss-arm running? joints: {sorted(now)})")
+ return 2
+ q_now = [now[f"{A.HW_ID}/{j}"] for j in A.ARM_JOINTS]
+ g_raw = now[f"{A.HW_ID}/{A.GRIPPER}"]
+ gap_now = A.sim_gripper_gap(g_raw) if a.sim else float(g_raw)
+ say("arm now: " + " ".join(f"{j} {A.rad_to_ticks(v)}" for j, v in zip(A.ARM_JOINTS, q_now)) + f" gap {gap_now * 1000:.0f} mm")
+ steps = list(cycle.steps)
+ off = max(abs(x - y) for x, y in zip(q_now, cycle.first_pose)) * A.TICKS_PER_RAD
+ if off > A.START_TOLERANCE_RAD * A.TICKS_PER_RAD:
+ if a.no_goto:
+ say(f"refused: the arm is {off:.0f} ticks from the cycle's first pose ({steps[0].start}) and --no-goto is set")
+ return 2
+ goto = A.goto_step(q_now, gap_now, cycle.first_pose, cycle.first_gap_m, speed=a.speed)
+ say(f"goto: {off:.0f} ticks from {steps[0].start}, {goto.duration_s:.1f} s first")
+ steps.insert(0, goto)
+ traj = A.cycle_trajectory(steps, sim=a.sim)
+ res = client.execute_trajectory(traj)
+ if res is None or res.status is not TrajectoryExecutionStatus.ACCEPTED:
+ say(f"refused by the coordinator: {getattr(res, 'status', None)} {getattr(res, 'message', '')}")
+ return 3
+ say(f"running: {len(traj.points)} points, {traj.duration:.1f} s")
+ t0 = time.time()
+ worst: dict[str, float] = {}
+ lag_since: dict[str, float] = {}
+ last_line = 0.0
+ while True:
+ t = time.time() - t0
+ cur = client.get_joint_positions() or {}
+ err = A.following_error_ticks(traj, t, cur)
+ for j, e in err.items():
+ worst[j] = max(worst.get(j, 0.0), e)
+ if e > a.max_lag:
+ lag_since.setdefault(j, t)
+ if t - lag_since[j] >= a.lag_seconds:
+ client.cancel_trajectory()
+ say(f"ARM LAG: {j} {e:.0f} ticks behind for {t - lag_since[j]:.1f} s at t = {t:.1f} s; "
+ f"trajectory cancelled, the coordinator holds the last commanded position")
+ _report(worst)
+ return 4
+ else:
+ lag_since.pop(j, None)
+ if t - last_line >= 2.0:
+ last_line = t
+ say(f" t {t:5.1f} s " + " ".join(f"{j.split('/')[-1]} {e:4.0f}" for j, e in err.items()))
+ if t >= traj.duration + 0.3:
+ break
+ time.sleep(POLL_S)
+ fin = client.get_joint_positions() or {}
+ say("done: " + " ".join(f"{j} {A.rad_to_ticks(fin.get(f'{A.HW_ID}/{j}', float('nan')))}" for j in A.ARM_JOINTS))
+ _report(worst)
+ return 0
+ finally:
+ try:
+ client.stop_rpc_client()
+ except Exception: # noqa: BLE001
+ pass
+
+
+def _report(worst: dict[str, float]) -> None:
+ say("worst following error: " + ", ".join(f"{j.split('/')[-1]} {e:.0f} ticks ({e / A.TICKS_PER_RAD * DEG:.1f} deg)" for j, e in worst.items()))
+
+
+if __name__ == "__main__":
+ sys.exit(main())