MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
"""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])
# ── G. the random walk and the clearance ──
print("G. the random walk and the clearance")
pr = W.Planner(GUARD, CAL_RUN, M, TOL).plan(25.0, 3, 0.25, "random")
rt = [w.ticks for w in pr.waypoints]
check("random: starts and ends at the drive pose, visits at least three cells",
rt[0] == DRIVE and rt[-1] == DRIVE and len({tuple(sorted(t.items())) for t in rt[1:-1]}) >= 3, f"{len(rt)} waypoints")
check("random: every waypoint is allowed by the guard", all(GUARD.allowed_ticks(t) for t in rt))
check("random: every move passes the segment check", all(not W.Planner.move_refused(P, rt[i - 1], rt[i]) for i in range(1, len(rt))))
check("random: two seeds give two different tours",
[w.ticks for w in W.Planner(GUARD, CAL_RUN, M, TOL).plan(25.0, 4, 0.25, "random").waypoints] != rt)
try:
pc = W.Planner(GUARD, CAL_RUN, M, TOL, clearance=1)
cells_c = [w.cell for w in pc.plan(25.0, 3, 0.25, "random").waypoints if w.cell is not None]
check("clearance 1: no waypoint cell touches a forbidden grid point of the table",
cells_c and all(not near_forbidden(GUARD, c) for c in cells_c), f"{len(cells_c)} cells")
except W.Refused as e:
check("clearance 1: the drive pose stays plannable", False, str(e)[:100])
r = run(["--random", "--speed", "1500", "--plan-only", "--table", str(table_path), "--calib", str(calib_path),
"--duration", "10", "--log", str(TMP / "g.jsonl")])
check("speed 1500 without --clearance is refused", r[0] == 1 and "needs --clearance" in (r[5] or ""), (r[5] or "").strip().splitlines()[0][:100] if r[5] else "")
# ── 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)