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.pynew file mode 100644index 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.pynew file mode 100644index 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.pyindex 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 fiveblueprints, 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 throughdimOS's own `dimos run` lookup (get_by_name) from atemporary dist-info built from pyproject.tomlG. 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.retgot = 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 # noqfrom dimos.robot.get_all_blueprints import get_by_name # noqa: E402listed = [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 doesw.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.WristCameradiff --git a/docs/arm.md b/docs/arm.mdindex 52a1456..7a71359 100644--- a/docs/arm.md+++ b/docs/arm.md@@ -457,3 +457,66 @@ python benches/bench_arm_drop_cold.pygrid step apart at most) with the guard, `segment_allowed` and the sweep'sbox 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.pynew file mode 100644index 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 outdiff --git a/moss_dimos/arm_feetech.py b/moss_dimos/arm_feetech.pynew file mode 100644index 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 Truediff --git a/moss_dimos/blueprints.py b/moss_dimos/blueprints.pyindex 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.tomlindex 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.pynew file mode 100644index 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())