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: tools/moss_arm_sweep.py, the slow tour of the arm's reach along the keep-out boundary, logged for a synchronized render

metroxb8382ccec808
diff --git a/benches/bench_arm_sweep_cold.py b/benches/bench_arm_sweep_cold.py
new file mode 100644
index 0000000..dc145f7
--- /dev/null
+++ b/benches/bench_arm_sweep_cold.py
@@ -0,0 +1,515 @@
+"""Cold bench for tools/moss_arm_sweep.py: the arm's tour along its keep-out
+zones, planned on the table and run on the fake bus. numpy only.
+
+Known input -> known output, in ticks, radians and seconds:
+
+ A. the speed model: 200 ticks at speed 200, acc 8 take 1.25 s (1.0 s at 200
+ ticks/s and 0.25 s of ramp), 20 ticks 0.316 s (no cruise); the joint is
+ within 25 ticks of its goal 0.25 s before the end; a fake servo told to
+ move 200 ticks reads its goal at 1.215 s (within half a tick of it) and
+ is within 25 ticks at 1.0 s; the
+ drive pose's ticks -> model radians -> ticks round trip
+ (pan 2857 -> +71.1 deg -> 2857)
+ B. the plan on the table (config/arm_keepout.json, MOSS_ARM_TABLE to point
+ elsewhere) with the calibration (config/arm_calibration.json when this
+ checkout holds one, MOSS_ARM_CALIB, else a fixture: drive and look as
+ docs/arm.md gives them, the travel of an SO-101). When the table forbids
+ that drive pose, the refusal names the part and the tour runs from a
+ stand-in: the free cell nearest the drive pose.
+ - it starts and ends at the drive pose; every waypoint is allowed, in a
+ cell whose 32 corners are free, inside the calibrated ranges by the
+ tolerance; every move passes segment_allowed and a box check done here
+ (every grid point between the two ends, each widened by the tolerance,
+ free); no joint moves more than one cell per move
+ - it hugs the boundary: most waypoints have a forbidden grid point within
+ one grid step of their cell
+ - it fits --duration within 20 %, never over (25 s and 40 s)
+ - the same seed gives the same tour, another seed another
+ C. refusals: a drive pose on a forbidden grid point (why() names the part),
+ a drive pose outside the table, a tolerance wider than half a grid step,
+ no calibration, a speed over the cap
+ D. the dry run (--dry-run --fast): exit 0; t strictly increasing; rad equal
+ to JointMap(ticks) on every line; every reading allowed or within the
+ tolerance of the drive pose; the last within the tolerance of the drive
+ pose; every waypoint in order; the gripper never written; the torque
+ only switched on; the run's time within 20 % of the plan's and of
+ --duration. --plan-only writes the same log and names the parts skirted.
+ E. faults mid-tour: Ctrl-C, a joint blocked mid-move (the timeout), a
+ waypoint the guard refuses slipped into the plan. The first goals after
+ the fault are the arm's readings (it stops where it is), every later
+ goal is reached by a move that passes segment_allowed and the box check,
+ the arm ends within the tolerance of the drive pose (exit 2). A knock
+ into a pose the guard refuses: no allowed move leaves it, the arm holds
+ there (exit 3), nothing is written after the hold.
+ F. an arm away from the drive pose is refused before anything is written.
+
+Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_arm_sweep_cold.py
+"""
+import atexit
+import io
+import json
+import math
+import os
+import sys
+import tempfile
+from contextlib import redirect_stdout
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "tools"))
+
+import moss_arm_sweep as W # noqa: E402
+from moss_dimos.arm_guard import ArmGuard # noqa: E402
+
+OK = 0
+KO = 0
+
+
+def check(label, ok, detail=""):
+ global OK, KO
+ print(f" {'OK ' if ok else 'KO '} {label}{' - ' + detail if detail else ''}")
+ if ok:
+ OK += 1
+ else:
+ KO += 1
+
+
+_TMP = tempfile.TemporaryDirectory(prefix="arm_sweep_")
+atexit.register(_TMP.cleanup)
+TMP = Path(_TMP.name)
+TOL = 25
+JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll")
+# drive and look as docs/arm.md gives them, the travel of an SO-101 arm as its calibration reads
+FIXTURE = {
+ "shoulder_pan": {"id": 1, "range_min": 702, "range_max": 3394},
+ "shoulder_lift": {"id": 2, "homing_offset": -1369, "range_min": 860, "range_max": 3236},
+ "elbow_flex": {"id": 3, "range_min": 890, "range_max": 3206},
+ "wrist_flex": {"id": 4, "range_min": 850, "range_max": 3202},
+ "wrist_roll": {"id": 5, "range_min": 171, "range_max": 3925},
+ "gripper": {"id": 6, "range_min": 1500, "range_max": 4095},
+ "poses": {
+ "drive": {"ticks": {"shoulder_pan": 2857, "shoulder_lift": 860, "elbow_flex": 3206, "wrist_flex": 868,
+ "wrist_roll": 3221, "gripper": 3674}},
+ "look": {"ticks": {"shoulder_pan": 2869, "shoulder_lift": 1057, "elbow_flex": 3208, "wrist_flex": 2018,
+ "wrist_roll": 3220, "gripper": 3674}},
+ },
+}
+
+
+def quiet(fn, *args, **kw):
+ buf = io.StringIO()
+ with redirect_stdout(buf):
+ out = fn(*args, **kw)
+ return out, buf.getvalue()
+
+
+def write_json(name, doc) -> Path:
+ path = TMP / name
+ path.write_text(json.dumps(doc))
+ return path
+
+
+def touched(g, lo, hi):
+ """Indices of the grid values [lo, hi] touches by the guard's rule (the
+ value below lo and above hi unless they sit on one), None outside."""
+ step = g[1] - g[0]
+ flo, fhi = (lo - g[0]) / step, (hi - g[0]) / step
+ if flo < -1e-6 or fhi > len(g) - 1 + 1e-6:
+ return None
+ a = int(round(flo)) if abs(flo - round(flo)) <= 1e-6 else int(math.floor(flo))
+ b = int(round(fhi)) if abs(fhi - round(fhi)) <= 1e-6 else int(math.floor(fhi)) + 1
+ return np.arange(a, b + 1)
+
+
+def box_ok(guard, a, b, drive):
+ """Every grid point between two poses (ticks), each end widened by the
+ tolerance unless it is the drive pose, is free."""
+ jm, idx = guard.joint_map, []
+ pad = TOL * 2 * math.pi / 4096
+ for j, g in zip(guard.joints, guard.grids):
+ qa, qb = jm.ticks_to_rad(j, a[j]), jm.ticks_to_rad(j, b[j])
+ pa = 0.0 if a == drive else pad
+ pb = 0.0 if b == drive else pad
+ t = touched(np.asarray(g), min(qa - pa, qb - pb), max(qa + pa, qb + pb))
+ if t is None:
+ return False
+ idx.append(t)
+ return not guard.occ[np.ix_(*idx)].any()
+
+
+def cell_of(guard, ticks):
+ q = guard.ticks_pose_to_rad(ticks)
+ return tuple(min(int(math.floor((q[j] - g[0]) / (g[1] - g[0]))), len(g) - 2) for j, g in zip(guard.joints, guard.grids))
+
+
+def corners_free(guard, c):
+ return not guard.occ[tuple(slice(x, x + 2) for x in c)].any()
+
+
+def near_forbidden(guard, c):
+ return bool(guard.occ[tuple(slice(max(x - 1, 0), x + 3) for x in c)].any())
+
+
+def stand_in_drive(guard, cal):
+ """The centre of the free cell, inside the ranges, nearest the drive pose."""
+ free = ~guard.occ
+ cells = free
+ for k in range(free.ndim):
+ lo = tuple(slice(0, -1) if i == k else slice(None) for i in range(free.ndim))
+ hi = tuple(slice(1, None) if i == k else slice(None) for i in range(free.ndim))
+ cells = cells[lo] & cells[hi]
+ q = guard.ticks_pose_to_rad(cal["poses"]["drive"]["ticks"])
+ f = np.array([(q[j] - g[0]) / (g[1] - g[0]) - 0.5 for j, g in zip(guard.joints, guard.grids)])
+ best, key = None, math.inf
+ for c in np.argwhere(cells):
+ ticks = {j: guard.joint_map.rad_to_ticks(j, (g[c[k]] + g[c[k] + 1]) / 2) for k, (j, g) in enumerate(zip(guard.joints, guard.grids))}
+ if any(not (cal[j]["range_min"] + 2 * TOL <= ticks[j] <= cal[j]["range_max"] - 2 * TOL) for j in JOINTS):
+ continue
+ d = np.abs(c - f)
+ k = d.max() * 1e3 + d.sum()
+ if k < key:
+ best, key = ticks, k
+ return best
+
+
+def read_log(path):
+ return [json.loads(line) for line in Path(path).read_text().splitlines()]
+
+
+def run(argv, hook=None):
+ """main() with the stdout caught: (exit status, the bus, the clock, the plan, the planner, the text)."""
+ seen = {}
+
+ def keep(bus, clk, plan, p):
+ seen.update(bus=bus, clk=clk, plan=plan, p=p)
+ if hook:
+ hook(bus, clk, plan, p)
+
+ code, text = quiet(W.main, argv, bus_hook=keep)
+ return code, seen.get("bus"), seen.get("clk"), seen.get("plan"), seen.get("p"), text
+
+
+def goal_writes(bus, after=-1.0):
+ """(t, {servo: goal}) per complete write of the five goals, in order."""
+ out, cur = [], {}
+ for t, sid, addr, v in bus.writes:
+ if addr != W.REG_GOAL or t <= after:
+ continue
+ cur[sid] = v
+ if len(cur) == 5:
+ out.append((t, dict(cur)))
+ cur = {}
+ return out
+
+
+def by_joint(ids, goals):
+ return {j: goals[s] for j, s in ids.items()}
+
+
+def log_zero(bus):
+ """The log's t = 0: the last torque write (the log opens right after it)."""
+ return [t for t, _, a, _ in bus.writes if a == W.REG_TORQUE][-1]
+
+
+def checked_move(a, b, p):
+ """A move the bench checks itself: segment_allowed and the box (the last
+ move from within the tolerance of the drive pose onto it excepted)."""
+ if p.parked(a) and b == p.drive:
+ return True
+ qa, qb = GUARD.ticks_pose_to_rad(a), GUARD.ticks_pose_to_rad(b)
+ return GUARD.segment_allowed(qa, qb) and box_ok(GUARD, a, b, p.drive)
+
+
+# ── inputs ──
+
+table_path = Path(os.environ.get("MOSS_ARM_TABLE", ROOT / "config" / "arm_keepout.json"))
+calib_env = os.environ.get("MOSS_ARM_CALIB")
+calib_path = Path(calib_env) if calib_env else ROOT / "config" / "arm_calibration.json"
+if calib_path.is_file():
+ CAL = json.loads(calib_path.read_text())
+ cal_src = str(calib_path)
+else:
+ CAL = FIXTURE
+ calib_path = write_json("calibration.json", FIXTURE)
+ cal_src = "the fixture (drive and look of docs/arm.md)"
+GUARD = ArmGuard.load(table_path, calib_path)
+print(f"table {table_path.name}: {GUARD.summary()}")
+print(f"calibration: {cal_src}")
+
+# ── A. the speed model ──
+print("A. the speed model")
+M = W.SpeedModel(200, 8, TOL)
+check("200 ticks at speed 200, acc 8: 1.25 s", abs(M.joint_time(200) - 1.25) < 1e-9, f"{M.joint_time(200):.4f} s")
+check("20 ticks: 2 sqrt(20 / 800) = 0.316 s", abs(M.joint_time(20) - 2 * math.sqrt(20 / 800)) < 1e-9, f"{M.joint_time(20):.4f} s")
+check("200 ticks: within 25 ticks 0.25 s before the end", abs(M.reach_time(200) - 1.0) < 1e-9, f"{M.reach_time(200):.4f} s")
+check("a joint already within the tolerance takes no time", M.reach_time(20) == 0.0)
+clk = W.VirtualClock()
+bus = W.FakeBus({1: 1000}, clk)
+bus.write1ByteTxRx(None, 1, W.REG_ACC, 8)
+bus.write2ByteTxRx(None, 1, W.REG_SPEED, 200)
+bus.write1ByteTxRx(None, 1, W.REG_TORQUE, 1)
+t0 = clk.now()
+bus.write2ByteTxRx(None, 1, W.REG_GOAL, 1200)
+t_in, t_at = None, None
+while clk.now() - t0 < 3.0:
+ v = bus.read2ByteTxRx(None, 1, W.REG_PRESENT)[0]
+ if t_in is None and abs(v - 1200) <= TOL:
+ t_in = clk.now() - t0
+ if v == 1200 and t_at is None:
+ t_at = clk.now() - t0
+ clk.sleep(0.005)
+# it reads its goal once within half a tick of it: 1.25 - sqrt(2 * 0.5 / 800) = 1.215 s
+check("a fake servo told to move 200 ticks reads its goal at 1.215 s (+-0.02)",
+ t_at is not None and abs(t_at - (1.25 - math.sqrt(1.0 / 800))) < 0.02, f"{t_at:.3f} s")
+check("and is within 25 ticks at 1.0 s (+-0.02)", t_in is not None and abs(t_in - 1.0) < 0.02, f"{t_in:.3f} s")
+d = CAL["poses"]["drive"]["ticks"]
+deg = math.degrees(GUARD.joint_map.ticks_to_rad("shoulder_pan", d["shoulder_pan"]))
+back = GUARD.joint_map.rad_to_ticks("shoulder_pan", math.radians(deg))
+check("drive's pan: ticks -> model degrees -> ticks", back == d["shoulder_pan"],
+ f"{d['shoulder_pan']} -> {deg:+.1f} deg -> {back}")
+
+# ── B. the plan ──
+print("B. the plan on the table")
+drive_q = GUARD.ticks_pose_to_rad(d)
+if GUARD.allowed(drive_q):
+ CAL_RUN, calib_run = CAL, calib_path
+ print(f" the table allows the calibration's drive pose: {GUARD.why(drive_q)}")
+else:
+ code, _, _, _, _, text = run(["--plan-only", "--table", str(table_path), "--calib", str(calib_path),
+ "--log", str(TMP / "refused.jsonl")])
+ part = GUARD.why(drive_q)
+ check("the table forbids the calibration's drive pose: the plan is refused and why() names the part",
+ code == 1 and "not allowed" in text and part in text, text.strip()[:160])
+ stand = stand_in_drive(GUARD, CAL)
+ CAL_RUN = json.loads(json.dumps(CAL))
+ CAL_RUN["poses"]["drive"]["ticks"].update(stand)
+ calib_run = write_json("calibration_stand_in.json", CAL_RUN)
+ print(f" the tour checks run from a stand-in drive pose: {stand}")
+DRIVE = {j: int(CAL_RUN["poses"]["drive"]["ticks"][j]) for j in JOINTS}
+P = W.Planner(GUARD, CAL_RUN, M, TOL)
+plan = P.plan(25.0, seed=0)
+wps = plan.waypoints
+ticks = [w.ticks for w in wps]
+check("it starts and ends at the drive pose", ticks[0] == DRIVE and ticks[-1] == DRIVE and len(wps) > 4,
+ f"{len(wps)} waypoints")
+inner = ticks[1:-1]
+check("every waypoint is allowed by the guard", all(GUARD.allowed_ticks(t) for t in ticks))
+check("every waypoint but the drive pose sits in a cell whose 32 corners are free",
+ all(corners_free(GUARD, cell_of(GUARD, t)) for t in inner))
+check("every waypoint is inside the calibrated ranges by the tolerance",
+ all(CAL_RUN[j]["range_min"] + TOL <= t[j] <= CAL_RUN[j]["range_max"] - TOL for t in inner for j in JOINTS))
+seg = [GUARD.segment_allowed(GUARD.ticks_pose_to_rad(a), GUARD.ticks_pose_to_rad(b)) for a, b in zip(ticks, ticks[1:])]
+check("every move passes segment_allowed", all(seg), f"{len(seg)} moves")
+boxes = [box_ok(GUARD, a, b, DRIVE) for a, b in zip(ticks, ticks[1:])]
+check("every move's box (both ends widened by the tolerance) holds free grid points only", all(boxes))
+steps = [np.abs(np.array(cell_of(GUARD, b)) - np.array(cell_of(GUARD, a))).max() for a, b in zip(ticks, ticks[1:])]
+check("no joint moves more than one cell per move", max(steps) <= 1, f"largest {max(steps)}")
+near = [near_forbidden(GUARD, cell_of(GUARD, t)) for t in inner]
+check("it hugs the boundary: 60 % or more of the waypoints have a forbidden grid point within one step",
+ np.mean(near) >= 0.6, f"{100 * np.mean(near):.0f} % ({sum(near)} of {len(near)})")
+for dur in (25.0, 40.0):
+ pl = plan if dur == 25.0 else P.plan(dur, seed=0)
+ check(f"the plan fits --duration {dur:g} s within 20 %, never over",
+ 0.8 * dur <= pl.total_s <= dur + 1e-9, f"{pl.total_s:.2f} s, {len(pl.waypoints)} waypoints")
+again = P.plan(25.0, seed=0)
+other = P.plan(25.0, seed=7)
+check("the same seed gives the same tour, another seed another",
+ [w.ticks for w in again.waypoints] == ticks and [w.ticks for w in other.waypoints] != ticks)
+skirted = [w.skirts for w in wps if w.skirts]
+check("the frontier waypoints name the part they skirt, as why() does",
+ skirted and all(w.skirts in (w.why or "") for w in wps if w.skirts), ", ".join(sorted(set(skirted)))[:160])
+
+# ── C. refusals ──
+print("C. refusals")
+idx = np.argwhere(GUARD.occ)
+mid = idx[len(idx) // 2]
+q_bad = {j: float(g[mid[k]]) for k, (j, g) in enumerate(zip(GUARD.joints, GUARD.grids))}
+bad = json.loads(json.dumps(CAL_RUN))
+bad["poses"]["drive"]["ticks"].update(GUARD.joint_map.pose_to_ticks(q_bad))
+code, _, _, _, _, text = run(["--plan-only", "--table", str(table_path), "--calib", str(write_json("bad.json", bad)),
+ "--log", str(TMP / "bad.jsonl")])
+part = GUARD.why(GUARD.ticks_pose_to_rad(bad["poses"]["drive"]["ticks"]))
+check("a drive pose on a forbidden grid point is refused, why() names the part",
+ code == 1 and "not allowed" in text and "forbidden" in part and part in text and not (TMP / "bad.jsonl").exists(),
+ text.strip()[:140])
+out = json.loads(json.dumps(CAL_RUN))
+out["poses"]["drive"]["ticks"]["wrist_roll"] = 4095
+code, _, _, _, _, text = run(["--plan-only", "--table", str(table_path), "--calib", str(write_json("out.json", out))])
+check("a drive pose outside the table is refused", code == 1 and "outside the table" in text, text.strip()[-90:])
+try:
+ W.Planner(GUARD, CAL_RUN, W.SpeedModel(200, 8, 120), 120)
+ check("a tolerance wider than half a grid step is refused", False)
+except W.Refused as e:
+ check("a tolerance wider than half a grid step is refused", "half a grid step" in str(e), str(e)[:90])
+code, _, _, _, _, text = run(["--plan-only", "--table", str(table_path), "--calib", str(TMP / "none.json")])
+check("no calibration: refused", code == 1 and "no calibration" in text)
+code, _, _, _, _, text = run(["--plan-only", "--speed", "900", "--table", str(table_path), "--calib", str(calib_run)])
+check("a speed over the cap is refused", code == 1 and "--speed 900" in text)
+
+# ── D. the dry run ──
+print("D. the dry run")
+BASE = ["--table", str(table_path), "--calib", str(calib_run)]
+log = TMP / "dry.jsonl"
+code, bus, clk, pl, p, text = run(BASE + ["--dry-run", "--fast", "--log", str(log)])
+lines = read_log(log)
+ts = [r["t"] for r in lines]
+ids = W.servo_ids(p)
+check("exit 0, back at the drive pose", code == 0 and "done" in text, text.strip().splitlines()[-1][:120])
+check("t strictly increasing", all(b > a for a, b in zip(ts, ts[1:])) and ts[0] == 0.0, f"{len(lines)} readings")
+err = max(abs(r["rad"][j] - GUARD.joint_map.ticks_to_rad(j, r["ticks"][j])) for r in lines for j in JOINTS)
+check("rad = JointMap(ticks) on every line", err < 1e-12 and all(set(r["rad"]) == set(JOINTS) for r in lines), f"{err:.1e}")
+check("every reading allowed by the guard or within the tolerance of the drive pose",
+ all(GUARD.allowed_ticks(r["ticks"]) or P.parked(r["ticks"]) for r in lines))
+check("the last reading within the tolerance of the drive pose", P.parked(lines[-1]["ticks"]), str(lines[-1]["ticks"]))
+wseen = [r["waypoint"] for r in lines]
+check("every waypoint in order", wseen == sorted(wseen) and set(wseen) == set(range(len(pl.waypoints))))
+check("the gripper (servo 6) never written", not [w for w in bus.writes if w[1] == W.GRIPPER_ID])
+torque = [(s, v) for _, s, a, v in bus.writes if a == W.REG_TORQUE]
+check("the torque only switched on, servos 1-5", sorted(torque) == [(s, 1) for s in range(1, 6)], str(torque))
+goals = [by_joint(ids, g) for _, g in goal_writes(bus)]
+check("the goals written: the arm's own reading first (a hold), then the plan's waypoints",
+ goals[0] == lines[0]["ticks"] and goals[1:] == [w.ticks for w in pl.waypoints[1:]])
+check("the run's time within 20 % of the plan's and of --duration",
+ abs(ts[-1] - pl.total_s) <= 0.2 * pl.total_s and abs(ts[-1] - 25.0) <= 5.0, f"run {ts[-1]:.2f} s, plan {pl.total_s:.2f} s")
+plog = TMP / "plan.jsonl"
+code, _, _, _, _, text = run(BASE + ["--plan-only", "--log", str(plog)])
+plines = read_log(plog)
+check("--plan-only: the tour printed with the parts skirted, the log written",
+ code == 0 and "skirts" in text and "planned log" in text and len(plines) == len(lines)
+ and all(b["t"] > a["t"] for a, b in zip(plines, plines[1:])), f"{len(plines)} readings")
+
+# ── E. faults ──
+print("E. faults mid-tour")
+
+
+def after_fault(bus, ids, lines, k_fault, p):
+ """The fault's checks: the hold, the way home, the end."""
+ rets = [r for r in lines if r["phase"] == "return"]
+ t_fault = rets[0]["t"] if rets else math.inf
+ t0 = log_zero(bus)
+ later = [(t - t0, by_joint(ids, g)) for t, g in goal_writes(bus) if t - t0 >= t_fault - 1e-3]
+ hold_ok = bool(later) and later[0][1] == rets[0]["ticks"]
+ moves = [g for _, g in later]
+ seg_ok = all(checked_move(a, b, p) for a, b in zip(moves, moves[1:]))
+ home = all(GUARD.allowed_ticks(r["ticks"]) or p.parked(r["ticks"]) for r in rets)
+ after_fault.moves = moves
+ return hold_ok, seg_ok, home, len(moves) - 1
+
+
+def ctrl_c(bus, clk, plan, p):
+ clk.alarm(0.45 * plan.total_s, lambda: setattr(clk.stop, "asked", True))
+
+
+code, bus, clk, pl, p, text = run(BASE + ["--dry-run", "--fast", "--log", str(TMP / "c.jsonl")], ctrl_c)
+lines = read_log(TMP / "c.jsonl")
+hold_ok, seg_ok, home, n = after_fault(bus, ids, lines, None, p)
+check("Ctrl-C: the arm stops at its reading", hold_ok and "Ctrl-C" in text)
+check("Ctrl-C: home through moves that pass segment_allowed and the box check, every reading allowed",
+ seg_ok and home, f"{n} moves home")
+check("Ctrl-C: exit 2, the last reading within the tolerance of the drive pose",
+ code == 2 and p.parked(lines[-1]["ticks"]), text.strip().splitlines()[-1][:100])
+
+K = len(pl.waypoints) // 2
+
+
+def block(bus, clk, plan, p):
+ a, b = plan.waypoints[K - 1].ticks, plan.waypoints[K].ticks
+ j = max(JOINTS, key=lambda j: abs(b[j] - a[j]))
+ sid = W.servo_ids(p)[j]
+ raw = bus.write2ByteTxRx
+ n = {"goals": 0}
+
+ def write(ph, s, addr, value):
+ if addr == W.REG_GOAL:
+ if n["goals"] // 5 == K and s == sid: # goal set 0 is the hold, set i is waypoint i
+ bus.servos[sid].block_at((a[j] + b[j]) / 2.0)
+ clk.alarm(clk.now() + 5.5, lambda: setattr(bus.servos[sid], "block", None))
+ n["goals"] += 1
+ return raw(ph, s, addr, value)
+
+ bus.write2ByteTxRx = write
+
+
+code, bus, clk, pl, p, text = run(BASE + ["--dry-run", "--fast", "--timeout", "5", "--log", str(TMP / "b.jsonl")], block)
+lines = read_log(TMP / "b.jsonl")
+hold_ok, seg_ok, home, n = after_fault(bus, ids, lines, K, p)
+check(f"a joint blocked on the way to waypoint {K}: a timeout fault, the arm stops at its reading",
+ hold_ok and f"waypoint {K} not reached" in text, [s for s in text.splitlines() if "fault" in s][:1][0][:110] if "fault" in text else text[-100:])
+check("blocked: back the way it came (to waypoint K - 1 first), home through checked moves, exit 2 at the drive pose",
+ len(after_fault.moves) > 1 and after_fault.moves[1] == pl.waypoints[K - 1].ticks
+ and seg_ok and home and code == 2 and p.parked(lines[-1]["ticks"]), f"{n} moves home")
+
+
+def knock(bus, clk, plan, p):
+ def hit():
+ pose = {j: int(round(bus.servos[s].pos)) for j, s in W.servo_ids(p).items()}
+ for j in JOINTS:
+ g = GUARD.grids[GUARD.joints.index(j)]
+ for v in g:
+ t = GUARD.joint_map.rad_to_ticks(j, float(v))
+ cand = dict(pose, **{j: t})
+ if not GUARD.allowed_ticks(cand) and abs(t - pose[j]) < 700:
+ s = W.servo_ids(p)[j]
+ bus.servos[s].pos = float(t)
+ hit.where = cand
+ return
+ hit.where = None
+ hit.where = None
+ clk.alarm(0.5 * plan.total_s, hit)
+ knock.hit = hit
+
+
+code, bus, clk, pl, p, text = run(BASE + ["--dry-run", "--fast", "--log", str(TMP / "k.jsonl")], knock)
+lines = read_log(TMP / "k.jsonl")
+rets = [r for r in lines if r["phase"] == "return"]
+t0 = log_zero(bus)
+after = [g for t, g in goal_writes(bus) if t - t0 >= rets[0]["t"] - 1e-3] if rets else []
+check("a knock into a pose the guard refuses: a fault, the arm holds there, exit 3",
+ knock.hit.where is not None and code == 3 and "refuses the arm's pose" in text and "holds there" in text,
+ text.strip().splitlines()[-1][:110])
+check("nothing written after the hold", len(after) == 1 and by_joint(ids, after[0]) == rets[0]["ticks"],
+ f"{len(after)} goal writes after the fault")
+
+# a waypoint the guard refuses, slipped into the plan
+plan2 = P.plan(25.0, seed=0)
+k = len(plan2.waypoints) // 2
+forbidden = None
+for c in np.argwhere(GUARD.occ):
+ q = {j: float(g[c[i]]) for i, (j, g) in enumerate(zip(GUARD.joints, GUARD.grids))}
+ t = GUARD.joint_map.pose_to_ticks(q)
+ if all(CAL_RUN[j]["range_min"] <= t[j] <= CAL_RUN[j]["range_max"] for j in JOINTS):
+ forbidden = t
+ break
+plan2.waypoints.insert(k, W.Waypoint(forbidden, "frontier"))
+clk = W.VirtualClock()
+bus = W.fake_bus(P, CAL_RUN, clk)
+arm = W.Arm(bus, bus, bus, W.servo_ids(P))
+code, text = quiet(W.execute, plan2, P, arm, clk, TMP / "t.jsonl", 12.0, W.Stop())
+lines = read_log(TMP / "t.jsonl")
+gs = [by_joint(ids, g) for _, g in goal_writes(bus)]
+check(f"a forbidden waypoint slipped in at {k}: refused before it is written, exit 2 at the drive pose",
+ forbidden not in gs and code == 2 and "refuses the move to waypoint" in text and P.parked(lines[-1]["ticks"]),
+ text.strip().splitlines()[0][:110])
+
+# ── F. the start ──
+print("F. the start")
+clk = W.VirtualClock()
+start = {s: DRIVE[j] for j, s in W.servo_ids(P).items()}
+start[1] += 4 * TOL
+bus = W.FakeBus(start, clk)
+arm = W.Arm(bus, bus, bus, W.servo_ids(P))
+try:
+ quiet(W.execute, plan, P, arm, clk, TMP / "f.jsonl", 12.0, W.Stop())
+ check("an arm 100 ticks away from the drive pose is refused", False)
+except W.Refused as e:
+ check("an arm 100 ticks away from the drive pose is refused, nothing written",
+ "not at the drive pose" in str(e) and not bus.writes and not (TMP / "f.jsonl").exists(), str(e)[:100])
+
+print()
+print(f"{OK} OK, {KO} KO")
+print("TEST PASSED" if KO == 0 else "TEST FAILED")
+_TMP.cleanup()
+sys.exit(1 if KO else 0)
diff --git a/docs/arm_sweep.md b/docs/arm_sweep.md
new file mode 100644
index 0000000..e685282
--- /dev/null
+++ b/docs/arm_sweep.md
@@ -0,0 +1,66 @@
+# The arm's boundary tour
+
+`tools/moss_arm_sweep.py` drives the arm slowly through poses the keep-out
+guard allows, each one next to a pose it forbids, and logs every reading of
+the five servos. It is made for film: the arm tours its reach for a set
+time, and the log drives a picture-in-picture render of the collision-zone
+map, frame for frame with the footage.
+
+## Filming
+
+1. Plan with nothing moving:
+
+ ```sh
+ python tools/moss_arm_sweep.py --plan-only --duration 25 --log plan.jsonl
+ ```
+
+ It prints the tour (every waypoint in model degrees, the time the speed
+ model gives, the rover part each one skirts) and writes `plan.jsonl`, the
+ log of a run on a fake bus, so the render can be made before the arm
+ moves. `--dry-run` runs the whole tool on that fake bus, on the wall
+ clock.
+2. Place the arm in the drive pose, start the camera, then the tour:
+
+ ```sh
+ python tools/moss_arm_sweep.py --duration 25 --log sweep.jsonl
+ ```
+
+ The arm leaves the drive pose, follows the edge of the keep-out zones for
+ the 25 seconds, and comes back to the drive pose. The same inputs give
+ the same tour; `--seed` picks another, `--speed` (200 by default, 17.6
+ degrees per second) sets the pace.
+3. Render `sweep.jsonl` into the picture-in-picture clip with the render
+ script that the NOTES file of the video folder names. Each line holds `t`
+ (seconds from the start), the ticks and the model radians of the five
+ joints, the waypoint the arm is heading to, and the phase (`tour`, or
+ `return` after a fault).
+
+Ctrl-C stops the tour: the arm stops where it is, then goes back to the
+drive pose through moves the guard allows.
+
+## What it refuses
+
+* A drive pose the guard forbids: no plan, and why() names the part.
+* An arm more than twice the tolerance (50 ticks) from the drive pose at
+ the start.
+* A speed over 400 (35 degrees per second), a tolerance wider than half a
+ grid step, a calibration without a drive pose.
+* During the run, a move or a reading the guard refuses, a waypoint not
+ reached in time and servos that stop answering are faults: the arm stops
+ and goes home through allowed moves, or holds where it is when no allowed
+ move leaves that pose.
+
+## What it never does
+
+* It never moves the gripper: servo 6 is never written. The keep-out table
+ counts the jaws at every opening.
+* It never turns the torque off: at the end the arm holds the drive pose.
+
+## Before the first tour
+
+The tour passes within one or two grid steps of the poses the table forbids,
+and the table knows its grid points only ([arm.md](arm.md), "What the table
+does not see"). The joint map behind it is the first alignment: check `drive`
+and `look` against the model first ([arm.md](arm.md), "The operator's
+check"), the pan most of all, whose zero is good to about 15 degrees, one
+grid step. Keep a hand on the E-stop.
diff --git a/tools/moss_arm_sweep.py b/tools/moss_arm_sweep.py
new file mode 100644
index 0000000..760892c
--- /dev/null
+++ b/tools/moss_arm_sweep.py
@@ -0,0 +1,1022 @@
+#!/usr/bin/env python3
+"""Tour the arm along the edge of its keep-out zones, slowly, and log every reading.
+
+The tool drives the SO-101 arm (servos 1 to 5; the gripper, servo 6, is never
+commanded) from the drive pose through poses the keep-out guard allows, each
+one next to poses the guard forbids, and back to the drive pose. Every
+reading of the five servos goes to a JSONL log, so a render of the
+collision-zone map can follow the arm frame by frame next to the film of it.
+
+ tools/moss_arm_sweep.py --plan-only --log plan.jsonl # print the tour, write the log the speed model gives
+ tools/moss_arm_sweep.py --dry-run --log dry.jsonl # the whole run on a fake bus, on the wall clock
+ tools/moss_arm_sweep.py --dry-run --fast --log dry.jsonl # the same on a virtual clock
+ tools/moss_arm_sweep.py --log sweep.jsonl # the arm
+
+Options
+ --port a /dev/serial/by-id path, or auto (default): MOSS_ARM_PORT, else
+ the one link /dev/serial/by-id/usb-1a86_USB_Single_Serial_*
+ --table the keep-out table (config/arm_keepout.json)
+ --calib the arm's calibration (config/arm_calibration.json): the drive
+ pose under "poses", the servo IDs and their ranges
+ --duration seconds the whole tour takes, the way back included (25)
+ --speed Goal_Speed written once to the five servos (200, at most 400)
+ --acc Acc written once to the five servos (8)
+ --tolerance ticks: a waypoint is reached when all five are this close (25)
+ --timeout seconds per waypoint before it counts as a fault (12)
+ --seed the random spread's seed: the same inputs give the same tour (0)
+ --spread how much the random spread weighs against the nearest move (0.25)
+ --log the JSONL log (arm_sweep.jsonl)
+
+The plan
+ The table's grid cuts the joint space into cells. A cell is free when every
+ grid point at its corners is free, and the guard allows any pose inside a
+ free cell. A waypoint is the centre of a free cell, rounded to ticks and
+ inside the servos' calibrated ranges by the tolerance. A frontier cell is a
+ free cell one grid step, along one joint, from a forbidden grid point: the
+ edge of a keep-out zone, as close as the table's grid can tell. The
+ tour starts at the drive pose and steps from cell to neighbouring frontier
+ cell (at most one cell on any joint per move), the nearest by the speed
+ model with a small random spread and a penalty for turning a joint back;
+ when no unvisited frontier cell is next to the arm, it takes the shortest
+ way through free cells to the nearest one. It comes back to the drive pose
+ by the shortest way through free cells, in time for --duration. Every move
+ passes two checks: segment_allowed on the straight move, and every grid
+ point of the box the move spans (both ends widened by the tolerance) must
+ be free. The second one is there because the five servos run at the same
+ speed: the arm's path is not straight in joint space, but it stays in that
+ box. The plan is refused when the drive pose is not allowed (the guard
+ says why), when the tolerance is wider than half a grid step, or when no
+ frontier cell is in reach in the time given.
+
+The speed model
+ Feetech STS servos read Goal_Speed in steps per second (one step is one
+ tick, 0.088 degree) and Acc in units of 100 steps per second squared. A
+ joint moving d ticks takes d / v + v / a when it reaches full speed, and
+ 2 sqrt(d / a) when it does not. The run writes the next waypoint as soon
+ as every joint is within the tolerance (the last ticks of a move overlap
+ the next one), so a move counts until its slowest joint is within the
+ tolerance, plus 0.05 s (one poll and the next write). --speed 200 is 17.6
+ degrees per second, --acc 8 is 800 ticks per second squared. The fake bus
+ moves its servos by the same law.
+
+The run
+ The arm must sit within twice the tolerance of the drive pose. Each
+ servo's goal is set to its present reading, then the speed and the
+ acceleration are written, then the torque comes on. For each waypoint the
+ five goals are written and the positions are read every 40 ms until all
+ five are within the tolerance, or the timeout passes. Every reading is one
+ line of the log:
+
+ {"t": seconds since the start, "ticks": {joint: reported ticks},
+ "rad": {joint: model radians, through the table's JointMap},
+ "waypoint": the index of the waypoint the arm is heading to,
+ "phase": "tour" or "return"}
+
+ A fault stops the tour: a reading the guard refuses (within the tolerance
+ of the drive pose excepted: the arm settles there, and the drive pose can
+ sit a few ticks from the table's edge), a waypoint not reached in time, a
+ move the guard refuses, servos that stop answering, or Ctrl-C. The arm
+ stops where it is (each goal set to its present reading), then goes back
+ to the drive pose through allowed moves only: to the waypoint it came from
+ (the one it was heading to when that move is refused), then the shortest
+ way home. When no allowed move leaves the pose it
+ stopped at, it holds there. The torque stays on in every case: at the end
+ the arm holds the drive pose. Exit status: 0 the tour done, 1 refused
+ (nothing moved), 2 a fault and the arm back at the drive pose, 3 a fault
+ and the arm holding where it stopped.
+
+ --plan-only prints the tour (each waypoint, the time the speed model gives,
+ and the rover part it skirts, from why() on the nearest forbidden grid
+ point) and writes the log of a run on the fake bus and a virtual clock, so
+ the render can be made before the arm moves. --dry-run runs the whole tool
+ on that fake bus, on the wall clock unless --fast.
+"""
+from __future__ import annotations
+
+import argparse
+import glob
+import heapq
+import itertools
+import json
+import math
+import os
+import signal
+import sys
+import time
+from dataclasses import dataclass, field
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "tools"))
+
+from moss_arm_calibrate import BAUD, read2 # noqa: E402 (the bus speed and the read retry the calibration uses)
+from moss_dimos.arm_guard import ArmGuard # noqa: E402
+
+TABLE = ROOT / "config" / "arm_keepout.json"
+CALIBRATION = ROOT / "config" / "arm_calibration.json"
+SERVO_IDS = {"shoulder_pan": 1, "shoulder_lift": 2, "elbow_flex": 3, "wrist_flex": 4, "wrist_roll": 5}
+GRIPPER_ID = 6 # never written by this tool
+REG_TORQUE, REG_ACC, REG_GOAL, REG_SPEED, REG_PRESENT = 40, 41, 42, 46, 56
+TICKS_PER_REV = 4096
+TICKS_PER_SPEED_UNIT = 1.0 # Goal_Speed: steps per second
+TICKS_PER_ACC_UNIT = 100.0 # Acc: 100 steps per second squared per unit
+SETTLE_S = 0.05 # per move: one poll and the next write
+POLL_S = 0.04
+MAX_SPEED = 400 # the tour hugs the rover: slow or not at all
+BUS_SILENT_S = 1.0 # no complete reading for this long is a fault
+REVERSE_S = 0.4 # the penalty, in seconds, for each joint a move turns back
+SEARCH_POPS = 50000 # the way to the nearest frontier cell gives up after this many cells
+PORT_GLOB = "/dev/serial/by-id/usb-1a86_USB_Single_Serial_*"
+ON_GRID = 1e-6
+
+
+class Refused(Exception):
+ """The plan or the run is refused before anything moves."""
+
+
+class Fault(Exception):
+ """The run stops: the arm holds, then goes home through allowed moves."""
+
+
+# ── the speed model ──
+
+@dataclass(frozen=True)
+class SpeedModel:
+ speed: int
+ acc: int
+ tol: int = 25
+
+ @property
+ def v(self) -> float:
+ return self.speed * TICKS_PER_SPEED_UNIT
+
+ @property
+ def a(self) -> float:
+ return self.acc * TICKS_PER_ACC_UNIT
+
+ def joint_time(self, d_ticks: float) -> float:
+ d = abs(float(d_ticks))
+ if d <= 0.0:
+ return 0.0
+ if d >= self.v * self.v / self.a:
+ return d / self.v + self.v / self.a
+ return 2.0 * math.sqrt(d / self.a)
+
+ def reach_time(self, d_ticks: float) -> float:
+ """Seconds until a joint moving d ticks is within the tolerance of its goal."""
+ d = abs(float(d_ticks))
+ if d <= self.tol:
+ return 0.0
+ peak = min(self.v, math.sqrt(self.a * d))
+ braking = peak * peak / (2.0 * self.a)
+ if self.tol <= braking:
+ return self.joint_time(d) - math.sqrt(2.0 * self.tol / self.a)
+ if peak >= self.v:
+ return self.joint_time(d) - peak / self.a - (self.tol - braking) / peak
+ return math.sqrt(2.0 * (d - self.tol) / self.a)
+
+ def move_time(self, a: dict, b: dict) -> float:
+ """A move: until its slowest joint is within the tolerance, plus a poll and a write."""
+ return max(self.reach_time(b[j] - a[j]) for j in a) + SETTLE_S
+
+
+# ── the plan ──
+
+@dataclass
+class Waypoint:
+ ticks: dict
+ kind: str # "drive", "frontier" or "pass"
+ cell: tuple | None = None
+ move_s: float = 0.0 # the speed model's time from the waypoint before
+ skirts: str | None = None # the rover part at the nearest forbidden grid point
+ why: str | None = None
+ steps_away: float | None = None
+
+
+@dataclass
+class Plan:
+ waypoints: list
+ duration: float
+ model: SpeedModel
+ tolerance: int
+ seed: int = 0
+ notes: list = field(default_factory=list)
+
+ @property
+ def total_s(self) -> float:
+ return sum(w.move_s for w in self.waypoints)
+
+ @property
+ def drive(self) -> dict:
+ return self.waypoints[0].ticks
+
+
+def _frac(grid: np.ndarray, v: float) -> float:
+ """v as a fractional index into a uniform grid (past the ends too)."""
+ return (float(v) - float(grid[0])) / (float(grid[1]) - float(grid[0]))
+
+
+def _touched(f: float, n: int) -> tuple[int, int] | None:
+ """The grid indices [lo, hi] a fractional index touches, None outside the grid."""
+ if f < -ON_GRID or f > n - 1 + ON_GRID:
+ return None
+ r = round(f)
+ if abs(f - r) <= ON_GRID:
+ return int(r), int(r)
+ return int(math.floor(f)), int(math.floor(f)) + 1
+
+
+def _shifted(arr: np.ndarray, o) -> np.ndarray:
+ """out[c] = arr[c + o], False where c + o leaves the array."""
+ out = np.zeros_like(arr)
+ src, dst = [], []
+ for ok, n in zip(o, arr.shape):
+ src.append(slice(max(ok, 0), n + min(ok, 0)))
+ dst.append(slice(max(-ok, 0), n - max(ok, 0)))
+ out[tuple(dst)] = arr[tuple(src)]
+ return out
+
+
+class Planner:
+ """The keep-out table as a graph of free cells, and the tour on it."""
+
+ def __init__(self, guard: ArmGuard, calibration: dict, model: SpeedModel, tolerance: int = 25):
+ self.guard, self.model, self.tol = guard, model, int(tolerance)
+ self.joints = tuple(guard.joints)
+ unknown = [j for j in self.joints if j not in SERVO_IDS]
+ if unknown:
+ raise Refused(f"the table has joints this tool does not drive: {', '.join(unknown)}")
+ self.jm = guard.joint_map
+ drive = ((calibration.get("poses") or {}).get("drive") or {}).get("ticks") or {}
+ missing = [j for j in self.joints if j not in drive]
+ if missing:
+ raise Refused(f"the calibration holds no drive pose for {', '.join(missing)} (poses.drive.ticks)")
+ self.drive = {j: int(drive[j]) for j in self.joints}
+ self.ranges = {}
+ for j in self.joints:
+ cal = calibration.get(j) if isinstance(calibration.get(j), dict) else {}
+ self.ranges[j] = (int(cal.get("range_min", 0)), int(cal.get("range_max", TICKS_PER_REV - 1)))
+ self.grids = [np.asarray(g, dtype=float) for g in guard.grids]
+ self.step_ticks = [abs(g[1] - g[0]) * TICKS_PER_REV / (2.0 * math.pi) for g in self.grids]
+ self.pad_rad = self.tol * 2.0 * math.pi / TICKS_PER_REV
+ for j, s in zip(self.joints, self.step_ticks):
+ if self.tol >= s / 2.0 - 1.0:
+ raise Refused(f"the tolerance ({self.tol} ticks) is wider than half a grid step on {j} ({s / 2.0:.0f} ticks)")
+
+ q_drive = self.rad(self.drive)
+ if not guard.allowed(q_drive):
+ raise Refused(f"the drive pose {self._fmt(self.drive)} is not allowed: {guard.why(q_drive)}")
+
+ occ = guard.occ
+ d = occ.ndim
+ cells, near = ~occ, occ.copy()
+ for k in range(d): # near: forbidden, or one grid step from a forbidden point along one joint
+ lo = tuple(slice(0, -1) if i == k else slice(None) for i in range(d))
+ hi = tuple(slice(1, None) if i == k else slice(None) for i in range(d))
+ near[lo] |= occ[hi]
+ near[hi] |= occ[lo]
+ for k in range(d): # a cell: the grid points k and k + 1 on every joint
+ lo = tuple(slice(0, -1) if i == k else slice(None) for i in range(d))
+ hi = tuple(slice(1, None) if i == k else slice(None) for i in range(d))
+ cells = cells[lo] & cells[hi]
+ near = near[lo] | near[hi]
+ self.cshape = cells.shape
+ self.cells = cells
+ self.centre = []
+ in_range = []
+ for j, g in zip(self.joints, self.grids):
+ c = np.array([self.jm.rad_to_ticks(j, (g[i] + g[i + 1]) / 2.0) for i in range(len(g) - 1)])
+ lo, hi = self.ranges[j]
+ self.centre.append(c)
+ in_range.append((c >= lo + self.tol) & (c <= hi - self.tol))
+ span = np.ones(self.cshape, dtype=bool)
+ for k, r in enumerate(in_range):
+ span &= r.reshape([-1 if i == k else 1 for i in range(d)])
+ self.cell_ok = cells & span
+ self.frontier = self.cell_ok & near
+
+ f = [_frac(g, q_drive[j]) for j, g in zip(self.joints, self.grids)]
+ self.c0 = tuple(int(min(max(math.floor(x), 0), n - 1)) for x, n in zip(f, self.cshape))
+ if not cells[self.c0]:
+ raise Refused(f"the drive pose sits on the edge of a cell with a forbidden corner: {guard.why(q_drive)}")
+
+ self.offsets = [o for o in itertools.product((-1, 0, 1), repeat=d) if any(o)]
+ self.offsets.sort(key=lambda o: sum(map(abs, o)))
+ box = {tuple([0] * d): cells}
+ for o in self.offsets:
+ k = max(i for i, v in enumerate(o) if v)
+ prev = tuple(0 if i == k else v for i, v in enumerate(o))
+ box[o] = box[prev] & _shifted(box[prev], tuple(o[i] if i == k else 0 for i in range(d)))
+ self.N = int(np.prod(self.cshape))
+ stride = [int(np.prod(self.cshape[k + 1:])) for k in range(d)]
+ self.offs = np.array(self.offsets, dtype=np.int64)
+ self.shift = self.offs @ np.array(stride, dtype=np.int64)
+ self.edge = np.stack([(box[o] & self.cell_ok & _shifted(self.cell_ok, o)).ravel() for o in self.offsets])
+ times = [self.model.reach_time(s + 1.0) for s in self.step_ticks] # + 1: centres rounded to ticks
+ self.cost = np.array([max(times[i] for i, v in enumerate(o) if v) + SETTLE_S for o in self.offsets])
+ # from the drive pose (exact, inside c0) to a cell next to c0, or to c0 itself
+ self.drive_next = {}
+ for o in [tuple([0] * d)] + self.offsets:
+ n = tuple(c + v for c, v in zip(self.c0, o))
+ if all(0 <= x < s for x, s in zip(n, self.cshape)) and box[o][self.c0] and self.cell_ok[n]:
+ self.drive_next[int(np.ravel_multi_index(n, self.cshape))] = o
+ del box
+ self.frontier_f = self.frontier.ravel()
+ self.dist = self._dist_home()
+
+ # ── ticks, radians, cells ──
+
+ def rad(self, ticks: dict) -> dict:
+ return {j: self.jm.ticks_to_rad(j, ticks[j]) for j in self.joints}
+
+ def deg(self, ticks: dict) -> dict:
+ return {j: math.degrees(v) for j, v in self.rad(ticks).items()}
+
+ def _fmt(self, ticks: dict) -> str:
+ return ", ".join(f"{j} {ticks[j]}" for j in self.joints)
+
+ def centre_ticks(self, n: int) -> dict:
+ c = np.unravel_index(n, self.cshape)
+ return {j: int(self.centre[k][c[k]]) for k, j in enumerate(self.joints)}
+
+ def to_drive_s(self, n: int) -> float:
+ return self.model.move_time(self.centre_ticks(n), self.drive) if n in self.drive_next else math.inf
+
+ def parked(self, ticks: dict) -> bool:
+ """Within the tolerance of the drive pose: where the arm settles at the end."""
+ return all(abs(int(ticks[j]) - self.drive[j]) <= self.tol for j in self.joints)
+
+ def box_free(self, a: dict, b: dict) -> bool:
+ """Every grid point of the box a move spans is free: each end widened by
+ the tolerance (the drive pose's end by nothing: it is the pose itself)."""
+ qa, qb = self.rad(a), self.rad(b)
+ pa = 0.0 if a == self.drive else self.pad_rad
+ pb = 0.0 if b == self.drive else self.pad_rad
+ idx = []
+ for j, g in zip(self.joints, self.grids):
+ lo = min(qa[j] - pa, qb[j] - pb)
+ hi = max(qa[j] + pa, qb[j] + pb)
+ tl, th = _touched(_frac(g, lo), len(g)), _touched(_frac(g, hi), len(g))
+ if tl is None or th is None:
+ return False
+ idx.append(np.arange(tl[0], th[1] + 1))
+ return not bool(self.guard.occ[np.ix_(*idx)].any())
+
+ def move_refused(self, a: dict, b: dict) -> str | None:
+ """None when the move from a to b passes every check, else why not."""
+ for name, p in (("from", a), ("to", b)):
+ if not self.guard.allowed(self.rad(p)):
+ return f"the pose it goes {name} is refused: {self.guard.why(self.rad(p))}"
+ if not self.guard.segment_allowed(self.rad(a), self.rad(b)):
+ return "segment_allowed refuses the straight move"
+ if not self.box_free(a, b):
+ return "the box the move spans holds a forbidden grid point"
+ return None
+
+ # ── the way home ──
+
+ def _dist_home(self) -> np.ndarray:
+ """Seconds from each cell's centre to the drive pose by the speed model,
+ moving through free cells, one cell per joint per move (inf: no way)."""
+ dist = np.full(self.N, np.inf)
+ for n in self.drive_next:
+ dist[n] = self.to_drive_s(n)
+ N = self.N
+ for _ in range(4 * sum(self.cshape)):
+ changed = False
+ for k in range(len(self.offsets)):
+ s, ok, c = int(self.shift[k]), self.edge[k], float(self.cost[k])
+ if s > 0:
+ cand, tgt, m = dist[s:] + c, dist[:N - s], ok[:N - s]
+ else:
+ cand, tgt, m = dist[:N + s] + c, dist[-s:], ok[-s:]
+ better = m & (cand < tgt - 1e-9)
+ if better.any():
+ tgt[better] = cand[better]
+ changed = True
+ if not changed:
+ break
+ return dist
+
+ def home_cells(self, n: int) -> list:
+ """The cells from n to the drive pose (n excluded), the drive pose as None."""
+ out, guard_steps = [], 0
+ while True:
+ direct = self.to_drive_s(n)
+ ok = self.edge[:, n]
+ nbr = np.clip(n + self.shift, 0, self.N - 1)
+ cand = np.where(ok, self.cost + self.dist[nbr], np.inf)
+ k = int(np.argmin(cand))
+ if direct <= cand[k] + 1e-9:
+ out.append(None)
+ return out
+ n = int(nbr[k])
+ out.append(n)
+ guard_steps += 1
+ if guard_steps > self.N or not math.isfinite(cand[k]):
+ raise Refused("no way home through free cells")
+
+ # ── the tour ──
+
+ def _nearest_frontier(self, cur: int | None, t: float, budget: float, visited: np.ndarray) -> list:
+ """The shortest way (cells after cur; cur None: the drive pose) to the
+ nearest unvisited frontier cell that still leaves the time to come
+ home; [] when none."""
+ if cur is None:
+ best = {n: self.to_drive_s(n) for n in self.drive_next}
+ parent = {n: None for n in self.drive_next}
+ else:
+ best, parent = {cur: 0.0}, {cur: None}
+ heap = [(d, n) for n, d in best.items()]
+ heapq.heapify(heap)
+ pops = 0
+ while heap and pops < SEARCH_POPS:
+ dn, n = heapq.heappop(heap)
+ if dn > best.get(n, math.inf) + 1e-12:
+ continue
+ pops += 1
+ if n != cur and self.frontier_f[n] and not visited[n] and t + dn + self.dist[n] <= budget + 1e-9:
+ path = []
+ while n is not None and n != cur:
+ path.append(n)
+ n = parent[n]
+ return path[::-1]
+ if t + dn > budget:
+ continue
+ ok = self.edge[:, n]
+ for k in np.flatnonzero(ok):
+ m = int(n + self.shift[k])
+ dm = dn + float(self.cost[k])
+ if dm < best.get(m, math.inf) - 1e-12:
+ best[m], parent[m] = dm, n
+ heapq.heappush(heap, (dm, m))
+ return []
+
+ def plan(self, duration: float, seed: int = 0, spread: float = 0.25) -> Plan:
+ budget = float(duration)
+ rng = np.random.default_rng(seed)
+ visited = np.zeros(self.N, dtype=bool)
+ cells = [] # the tour's cells, in order
+ t, prev_o = 0.0, np.zeros(len(self.joints), dtype=np.int64)
+ cur = None
+ while True:
+ if cur is None: # leaving the drive pose
+ nbr = np.array(sorted(self.drive_next), dtype=np.int64)
+ if not len(nbr):
+ break
+ costs = np.array([self.to_drive_s(int(n)) for n in nbr])
+ offs = np.array([self.drive_next[int(n)] for n in nbr], dtype=np.int64)
+ ok = np.ones(len(nbr), dtype=bool)
+ else:
+ nbr = np.clip(cur + self.shift, 0, self.N - 1)
+ costs, offs, ok = self.cost, self.offs, self.edge[:, cur]
+ m = ok & self.frontier_f[nbr] & ~visited[nbr] & (t + costs + self.dist[nbr] <= budget + 1e-9)
+ if m.any():
+ idx = np.flatnonzero(m)
+ turns = ((offs[idx] * prev_o) < 0).sum(axis=1)
+ score = costs[idx] * (1.0 + spread * rng.random(len(idx))) + REVERSE_S * turns
+ k = int(idx[int(np.argmin(score))])
+ step = [int(nbr[k])]
+ else:
+ step = self._nearest_frontier(cur, t, budget, visited)
+ if not step:
+ break
+ for n in step:
+ if cur is None:
+ t += self.to_drive_s(n)
+ o = np.array(self.drive_next[n], dtype=np.int64)
+ else:
+ o = np.array(np.unravel_index(n, self.cshape)) - np.array(np.unravel_index(cur, self.cshape))
+ t += float(self.cost[self.offsets.index(tuple(int(x) for x in o))])
+ visited[n] = True
+ cells.append(n)
+ prev_o, cur = o, n
+ if not any(self.frontier_f[n] for n in cells):
+ raise Refused(f"no frontier cell is in reach of the drive pose in {duration:g} s "
+ f"at speed {self.model.speed}")
+ cells += self.home_cells(cells[-1])
+ wps = [Waypoint(dict(self.drive), "drive")]
+ for n in cells:
+ if n is None:
+ w = Waypoint(dict(self.drive), "drive")
+ else:
+ w = Waypoint(self.centre_ticks(n), "frontier" if self.frontier_f[n] else "pass",
+ tuple(int(x) for x in np.unravel_index(n, self.cshape)))
+ w.move_s = self.model.move_time(wps[-1].ticks, w.ticks)
+ wps.append(w)
+ for w in wps:
+ if w.kind == "frontier":
+ self._skirt(w)
+ plan = Plan(wps, budget, self.model, self.tol, seed)
+ for i in range(1, len(wps)):
+ why = self.move_refused(wps[i - 1].ticks, wps[i].ticks)
+ if why:
+ raise Refused(f"the planned move to waypoint {i} fails its check ({why})")
+ return plan
+
+ def _skirt(self, w: Waypoint) -> None:
+ """The forbidden grid point nearest the waypoint (grid steps, the most
+ off joint first), why() there, and the part it names."""
+ c = w.cell
+ lo = [max(x - 1, 0) for x in c]
+ hi = [min(x + 3, n) for x, n in zip(c, self.guard.shape)]
+ sub = self.guard.occ[tuple(slice(a, b) for a, b in zip(lo, hi))]
+ pts = np.argwhere(sub) + np.array(lo)
+ if not len(pts):
+ return
+ f = np.array([_frac(g, v) for g, v in zip(self.grids, self.rad(w.ticks).values())])
+ diff = np.abs(pts - f)
+ key = diff.max(axis=1) * 1e3 + np.sqrt((diff ** 2).sum(axis=1))
+ p = tuple(int(x) for x in pts[int(np.argmin(key))])
+ q = {j: float(g[p[k]]) for k, (j, g) in enumerate(zip(self.joints, self.grids))}
+ w.why = self.guard.why(q)
+ w.steps_away = float(diff[int(np.argmin(key))].max())
+ k = int(self.guard.cell_part[p])
+ names = list(getattr(self.guard, "part_names", []) or [])
+ w.skirts = names[k - 1] if 1 <= k <= len(names) else "the rover"
+
+
+# ── clocks and the fake bus ──
+
+class RealClock:
+ def now(self) -> float:
+ return time.monotonic()
+
+ def sleep(self, s: float) -> None:
+ time.sleep(s)
+
+ def bus(self, s: float) -> None:
+ pass
+
+
+class VirtualClock:
+ """Time that moves only when the tool sleeps or talks on the bus. alarm(t, f)
+ calls f once the clock passes t (a bench's Ctrl-C, a knock on the arm)."""
+
+ def __init__(self):
+ self.t = 0.0
+ self.alarms = []
+
+ def now(self) -> float:
+ return self.t
+
+ def sleep(self, s: float) -> None:
+ self.t += s
+ self._ring()
+
+ def bus(self, s: float) -> None:
+ self.t += s
+ self._ring()
+
+ def alarm(self, at: float, fn) -> None:
+ self.alarms.append((at, fn))
+
+ def _ring(self) -> None:
+ due = [a for a in self.alarms if a[0] <= self.t]
+ self.alarms = [a for a in self.alarms if a[0] > self.t]
+ for _, fn in due:
+ fn()
+
+
+class _FakeServo:
+ def __init__(self, pos: float, limits=(0, TICKS_PER_REV - 1)):
+ self.pos, self.vel, self.goal = float(pos), 0.0, int(round(pos))
+ self.speed, self.acc, self.torque = 0, 0, 0
+ self.limits = limits
+ self.block = None # (tick, side): an obstacle the joint cannot pass, on that side of it
+
+ def block_at(self, tick: float) -> None:
+ self.block = (float(tick), 1.0 if tick >= self.pos else -1.0)
+
+ def step(self, dt: float) -> None:
+ if not self.torque:
+ self.vel = 0.0
+ return
+ vmax = self.speed * TICKS_PER_SPEED_UNIT if self.speed else 3000.0
+ amax = self.acc * TICKS_PER_ACC_UNIT if self.acc else 1e6
+ e = self.goal - self.pos
+ if abs(e) < 0.5 and abs(self.vel) <= amax * dt:
+ self.pos, self.vel = float(self.goal), 0.0
+ return
+ want = math.copysign(min(vmax, math.sqrt(2.0 * amax * abs(e))), e)
+ self.vel += max(-amax * dt, min(amax * dt, want - self.vel))
+ new = self.pos + self.vel * dt
+ if self.block is not None and (new - self.block[0]) * self.block[1] > 0:
+ new, self.vel = self.block[0], 0.0
+ self.pos = new
+
+
+class FakeBus:
+ """The calls this tool makes on scservo_sdk (PortHandler, PacketHandler and
+ COMM_SUCCESS in one object), on servos that follow the speed model."""
+
+ COMM_SUCCESS = 0
+
+ def __init__(self, start: dict, clock, limits: dict | None = None, latency: float = 0.0012):
+ self.clock, self.latency = clock, latency
+ self.servos = {int(s): _FakeServo(p, (limits or {}).get(int(s), (0, TICKS_PER_REV - 1))) for s, p in start.items()}
+ self.writes = [] # (t, id, register, value)
+ self.last = clock.now()
+
+ def openPort(self): # noqa: N802 - the SDK's names
+ return True
+
+ def setBaudRate(self, baud): # noqa: N802
+ return True
+
+ def closePort(self): # noqa: N802
+ pass
+
+ def _tx(self) -> None:
+ self.clock.bus(self.latency)
+ now = self.clock.now()
+ left = now - self.last
+ while left > 1e-12:
+ dt = min(0.002, left)
+ for s in self.servos.values():
+ s.step(dt)
+ left -= dt
+ self.last = now
+
+ def ping(self, ph, sid):
+ self._tx()
+ return 0, (0 if sid in self.servos else -1001), 0
+
+ def read2ByteTxRx(self, ph, sid, addr): # noqa: N802
+ self._tx()
+ s = self.servos.get(sid)
+ if s is None:
+ return 0, -1001, 0
+ return {REG_PRESENT: int(round(s.pos)), REG_GOAL: s.goal, REG_SPEED: s.speed}.get(addr, 0), 0, 0
+
+ def write1ByteTxRx(self, ph, sid, addr, value): # noqa: N802
+ self._tx()
+ self.writes.append((self.clock.now(), sid, addr, int(value)))
+ s = self.servos.get(sid)
+ if s is None:
+ return -1001, 0
+ if addr == REG_TORQUE:
+ s.torque = int(value)
+ elif addr == REG_ACC:
+ s.acc = int(value)
+ return 0, 0
+
+ def write2ByteTxRx(self, ph, sid, addr, value): # noqa: N802
+ self._tx()
+ self.writes.append((self.clock.now(), sid, addr, int(value)))
+ s = self.servos.get(sid)
+ if s is None:
+ return -1001, 0
+ if addr == REG_GOAL:
+ s.goal = int(min(max(int(value), s.limits[0]), s.limits[1]))
+ elif addr == REG_SPEED:
+ s.speed = int(value)
+ return 0, 0
+
+
+# ── the run ──
+
+class Stop:
+ """Ctrl-C asks for a stop; the run checks between two bus calls."""
+
+ def __init__(self):
+ self.asked = False
+
+ def __call__(self, *_):
+ self.asked = True
+
+
+class Arm:
+ def __init__(self, scs, ph, pk, ids: dict):
+ self.scs, self.ph, self.pk, self.ids = scs, ph, pk, ids
+
+ def read(self) -> dict | None:
+ out = {}
+ for j, sid in self.ids.items():
+ v = read2(self.scs, self.ph, self.pk, sid, REG_PRESENT)
+ if v is None:
+ return None
+ out[j] = -(v & 0x7FFF) if v & 0x8000 else int(v)
+ return out
+
+ def _write(self, sid: int, addr: int, value: int, width: int) -> None:
+ if sid == GRIPPER_ID or sid not in self.ids.values():
+ raise Fault(f"servo {sid} is not one this tool drives")
+ fn = self.pk.write1ByteTxRx if width == 1 else self.pk.write2ByteTxRx
+ for _ in range(3):
+ try:
+ comm, _err = fn(self.ph, sid, addr, int(value))
+ if comm == self.scs.COMM_SUCCESS:
+ return
+ except Exception: # noqa: BLE001 - a short packet inside the SDK
+ pass
+ raise Fault(f"servo {sid} did not take register {addr} = {value}")
+
+ def goals(self, ticks: dict) -> None:
+ for j, sid in self.ids.items():
+ self._write(sid, REG_GOAL, int(ticks[j]), 2)
+
+ def start(self, speed: int, acc: int) -> None:
+ for sid in self.ids.values():
+ self._write(sid, REG_ACC, acc, 1)
+ self._write(sid, REG_SPEED, speed, 2)
+ for sid in self.ids.values():
+ self._write(sid, REG_TORQUE, 1, 1)
+
+
+class Log:
+ def __init__(self, path, planner: Planner, clock, t0: float):
+ self.fh = open(path, "w")
+ self.p, self.clock, self.t0 = planner, clock, t0
+ self.lines = 0
+ self.last = None
+
+ def line(self, pose: dict, waypoint: int, phase: str) -> None:
+ self.last = pose
+ rad = {j: self.p.jm.ticks_to_rad(j, pose[j]) for j in self.p.joints}
+ self.fh.write(json.dumps({"t": round(self.clock.now() - self.t0, 4), "ticks": {j: int(pose[j]) for j in self.p.joints},
+ "rad": rad, "waypoint": waypoint, "phase": phase}) + "\n")
+ self.fh.flush()
+ self.lines += 1
+
+ def close(self) -> None:
+ self.fh.close()
+
+
+def _reach(arm: Arm, p: Planner, log: Log, clock, stop: Stop, target: dict, i: int, phase: str, timeout: float) -> None:
+ t_end = clock.now() + timeout
+ heard = clock.now()
+ while True:
+ pose = arm.read()
+ now = clock.now()
+ if pose is not None:
+ heard = now
+ log.line(pose, i, phase)
+ if not (p.parked(pose) or p.guard.allowed_ticks(pose)):
+ raise Fault(f"the guard refuses the arm's pose {p._fmt(pose)}: {p.guard.why_ticks(pose)}")
+ if all(abs(pose[j] - target[j]) <= p.tol for j in p.joints):
+ return
+ elif now - heard > BUS_SILENT_S:
+ raise Fault(f"no complete reading of the five servos for {BUS_SILENT_S:g} s")
+ if stop.asked:
+ raise Fault("stopped by the operator (Ctrl-C)")
+ if now > t_end:
+ off = {j: pose[j] - target[j] for j in p.joints} if pose else {}
+ raise Fault(f"waypoint {i} not reached in {timeout:g} s (off by {off} ticks)")
+ clock.sleep(POLL_S)
+
+
+def _hold(arm: Arm, log: Log) -> dict | None:
+ """Stop where the arm is: each goal set to its present reading."""
+ pose = None
+ for _ in range(5):
+ pose = arm.read()
+ if pose is not None:
+ break
+ pose = pose or log.last
+ if pose is not None:
+ try:
+ arm.goals(pose)
+ except Fault:
+ pass # a servo that takes no goal keeps the last one, a checked waypoint
+ return pose
+
+
+def _route_home(p: Planner, plan: Plan, pose: dict, around: list) -> list | None:
+ """Waypoints (ticks) from pose to the drive pose through allowed moves, or
+ None: back to the waypoint the arm left (what stopped it may lie ahead),
+ else on to the one it was heading to, then the shortest way home."""
+ if p.parked(pose):
+ return [dict(p.drive)]
+ if not p.guard.allowed_ticks(pose):
+ return None
+ for i in around:
+ w = plan.waypoints[i]
+ if p.move_refused(pose, w.ticks):
+ continue
+ if w.kind == "drive":
+ return [dict(p.drive)]
+ n = int(np.ravel_multi_index(w.cell, p.cshape))
+ return [w.ticks] + [dict(p.drive) if c is None else p.centre_ticks(c) for c in p.home_cells(n)]
+ return None
+
+
+def execute(plan: Plan, p: Planner, arm: Arm, clock, log_path, timeout: float = 12.0, stop: Stop | None = None,
+ say=print) -> int:
+ """Run the plan on the arm. Returns the exit status (see the docstring)."""
+ stop = stop or Stop()
+ present = arm.read()
+ if present is None:
+ raise Refused("the five servos do not answer")
+ off = {j: present[j] - p.drive[j] for j in p.joints}
+ if any(abs(v) > 2 * p.tol for v in off.values()):
+ raise Refused(f"the arm is not at the drive pose: it reads {p._fmt(present)}, "
+ f"{', '.join(f'{j} {v:+d}' for j, v in off.items() if abs(v) > 2 * p.tol)} ticks away")
+ arm.goals(present) # hold where it is before the torque comes on
+ arm.start(plan.model.speed, plan.model.acc)
+ log = Log(log_path, p, clock, clock.now())
+ try:
+ return _run(plan, p, arm, clock, log, timeout, stop, say, present)
+ finally:
+ log.close()
+
+
+def _run(plan: Plan, p: Planner, arm: Arm, clock, log: Log, timeout: float, stop: Stop, say, present: dict) -> int:
+ wps = plan.waypoints
+ at, i = 0, 0
+ try:
+ log.line(present, 0, "tour")
+ for i in range(1, len(wps)):
+ why = p.move_refused(wps[at].ticks, wps[i].ticks)
+ if why:
+ raise Fault(f"the guard refuses the move to waypoint {i}: {why}")
+ arm.goals(wps[i].ticks)
+ _reach(arm, p, log, clock, stop, wps[i].ticks, i, "tour", timeout)
+ at = i
+ say(f"done: back at the drive pose after {clock.now() - log.t0:.1f} s, {log.lines} readings in {log.fh.name}; "
+ "torque on, the arm holds the drive pose")
+ return 0
+ except Fault as e:
+ say(f"fault on the way to waypoint {i}: {e}")
+ stop.asked = False
+ pose = _hold(arm, log)
+ if pose is None:
+ say("the servos do not answer: nothing more is commanded")
+ return 3
+ log.line(pose, i, "return")
+ route = _route_home(p, plan, pose, [at, i] if i != at else [at])
+ if route is None:
+ say(f"no allowed move leaves the pose {p._fmt(pose)} ({p.guard.why_ticks(pose)}): "
+ "the arm holds there, torque on")
+ return 3
+ say(f"going home through {len(route)} allowed move(s)")
+ here = pose
+ try:
+ for k, target in enumerate(route, start=1):
+ if not (p.parked(here) and target == p.drive):
+ why = p.move_refused(here, target)
+ if why:
+ raise Fault(f"the guard refuses the way home at move {k}: {why}")
+ arm.goals(target)
+ _reach(arm, p, log, clock, stop, target, k, "return", timeout)
+ here = target
+ except Fault as e:
+ pose = _hold(arm, log)
+ say(f"fault on the way home: {e}; the arm holds at {p._fmt(pose) if pose else 'its last pose'}, torque on")
+ return 3
+ say(f"back at the drive pose after {clock.now() - log.t0:.1f} s, {log.lines} readings in {log.fh.name}; torque on")
+ return 2
+
+
+# ── the command line ──
+
+def resolve_port(arg: str) -> str:
+ if arg and arg != "auto":
+ return arg
+ env = os.environ.get("MOSS_ARM_PORT")
+ if env:
+ return env
+ found = sorted(glob.glob(PORT_GLOB))
+ if len(found) != 1:
+ raise Refused(f"{len(found)} links match {PORT_GLOB} ({', '.join(found) or 'none'}): give --port or MOSS_ARM_PORT")
+ return found[0]
+
+
+def open_bus(port: str):
+ import scservo_sdk as scs
+ ph = scs.PortHandler(port)
+ if not ph.openPort():
+ raise Refused(f"cannot open {port}")
+ ph.setBaudRate(BAUD)
+ return scs, ph, scs.PacketHandler(0)
+
+
+def servo_ids(p: Planner) -> dict:
+ ids = {}
+ for j in p.joints:
+ sid = p.jm.joints[j].servo_id if j in p.jm.joints else None
+ ids[j] = int(sid if sid is not None else SERVO_IDS[j])
+ if GRIPPER_ID in ids.values():
+ raise Refused(f"servo {GRIPPER_ID} is the gripper: the calibration gives it to {[j for j, s in ids.items() if s == GRIPPER_ID]}")
+ return ids
+
+
+def fake_bus(p: Planner, calibration: dict, clock) -> FakeBus:
+ start = {sid: p.drive[j] for j, sid in servo_ids(p).items()}
+ grip = (((calibration.get("poses") or {}).get("drive") or {}).get("ticks") or {}).get("gripper", 2048)
+ start[GRIPPER_ID] = int(grip)
+ limits = {sid: p.ranges[j] for j, sid in servo_ids(p).items()}
+ return FakeBus(start, clock, limits)
+
+
+def print_plan(plan: Plan, p: Planner, full: bool, say=print) -> None:
+ m = p.guard.meta.get("model", {}) or {}
+ say(f"table: {p.guard.summary()}; model {m.get('path')}")
+ say(f"drive: {p._fmt(p.drive)} ticks; {p.guard.why_ticks(p.drive)}")
+ say(f"speed model: {plan.model.v:.0f} ticks/s ({plan.model.v * 360.0 / TICKS_PER_REV:.1f} deg/s), "
+ f"{plan.model.a:.0f} ticks/s2, {SETTLE_S} s per move; tolerance {plan.tolerance} ticks")
+ t = 0.0
+ if full:
+ say(" # t s kind " + " ".join(f"{j[:12]:>12s}" for j in p.joints) + " (model degrees)")
+ for i, w in enumerate(plan.waypoints):
+ t += w.move_s
+ if not full:
+ continue
+ deg = p.deg(w.ticks)
+ say(f"{i:2d} {t:7.2f} {w.kind:8s} " + " ".join(f"{deg[j]:+12.1f}" for j in p.joints)
+ + (f" skirts {w.skirts} ({w.steps_away:.1f} grid steps)" if w.skirts else ""))
+ if w.why:
+ say(f" {w.why}")
+ parts = {}
+ for w in plan.waypoints:
+ if w.skirts:
+ parts[w.skirts] = parts.get(w.skirts, 0) + 1
+ kinds = [w.kind for w in plan.waypoints]
+ say(f"tour: {len(kinds)} waypoints ({kinds.count('frontier')} on the frontier, {kinds.count('pass')} passing, "
+ f"drive at both ends), {plan.total_s:.1f} s by the speed model for --duration {plan.duration:g} s")
+ say("skirts: " + ("; ".join(f"{n} x{c}" for n, c in sorted(parts.items(), key=lambda kv: -kv[1])) or "nothing"))
+
+
+def load(table, calib) -> tuple[ArmGuard, dict]:
+ if not Path(calib).is_file():
+ raise Refused(f"no calibration at {calib}: the drive pose and the servo ranges come from it "
+ "(tools/moss_arm_calibrate.py, docs/arm.md)")
+ try:
+ calibration = json.loads(Path(calib).read_text())
+ except ValueError as e:
+ raise Refused(f"the calibration {calib} does not parse: {e}") from e
+ try:
+ guard = ArmGuard.load(table, calib)
+ except (OSError, ValueError, KeyError) as e:
+ raise Refused(f"the table {table} does not load: {e}") from e
+ return guard, calibration
+
+
+def main(argv=None, bus_hook=None, say=print) -> int:
+ ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
+ ap.add_argument("--port", default="auto")
+ ap.add_argument("--table", default=str(TABLE))
+ ap.add_argument("--calib", default=str(CALIBRATION))
+ ap.add_argument("--duration", type=float, default=25.0)
+ ap.add_argument("--speed", type=int, default=200)
+ ap.add_argument("--acc", type=int, default=8)
+ ap.add_argument("--tolerance", type=int, default=25)
+ ap.add_argument("--timeout", type=float, default=12.0)
+ ap.add_argument("--seed", type=int, default=0)
+ ap.add_argument("--spread", type=float, default=0.25)
+ ap.add_argument("--log", default="arm_sweep.jsonl")
+ ap.add_argument("--dry-run", action="store_true", help="a fake bus instead of the arm")
+ ap.add_argument("--fast", action="store_true", help="with --dry-run: a virtual clock")
+ ap.add_argument("--plan-only", action="store_true", help="print the tour, write the log the speed model gives")
+ a = ap.parse_args(argv)
+ if not 1 <= a.speed <= MAX_SPEED:
+ say(f"refused: --speed {a.speed}: 1 to {MAX_SPEED}")
+ return 1
+ if not 1 <= a.acc <= 254:
+ say(f"refused: --acc {a.acc}: 1 to 254")
+ return 1
+ try:
+ guard, calibration = load(a.table, a.calib)
+ p = Planner(guard, calibration, SpeedModel(a.speed, a.acc, a.tolerance), a.tolerance)
+ plan = p.plan(a.duration, a.seed, a.spread)
+ except Refused as e:
+ say(f"refused: {e}")
+ return 1
+ print_plan(plan, p, full=a.plan_only, say=say)
+ try:
+ quiet = (lambda *_: None) if a.plan_only else say
+ if a.plan_only or a.dry_run:
+ clk = VirtualClock() if (a.plan_only or a.fast) else RealClock()
+ bus = fake_bus(p, calibration, clk)
+ if bus_hook:
+ bus_hook(bus, clk, plan, p)
+ scs = ph = pk = bus
+ else:
+ clk = RealClock()
+ scs, ph, pk = open_bus(resolve_port(a.port))
+ arm = Arm(scs, ph, pk, servo_ids(p))
+ stop = Stop()
+ old = signal.signal(signal.SIGINT, stop) if not a.plan_only else None
+ if isinstance(clk, VirtualClock):
+ clk.stop = stop
+ try:
+ code = execute(plan, p, arm, clk, a.log, a.timeout, stop, say=quiet)
+ finally:
+ if old is not None:
+ signal.signal(signal.SIGINT, old)
+ ph.closePort()
+ except Refused as e:
+ say(f"refused: {e}")
+ return 1
+ if a.plan_only:
+ lines = Path(a.log).read_text().splitlines()
+ end = json.loads(lines[-1])["t"] if lines else 0.0
+ say(f"planned log: {a.log}, {len(lines)} readings over {end:.1f} s (a run on the fake bus, t from the speed model)")
+ return code
+
+
+if __name__ == "__main__":
+ sys.exit(main())