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
46.7 KB
Raw Blame
#!/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)
  --random      a random walk over the allowed cells (any direction, unvisited cells
                first, the way home always kept) instead of the boundary tour
  --clearance   grid steps kept clear of any forbidden point (0); a run above speed
                400 needs 1 or more, because the table knows poses, not overshoot
  --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
SLOW_SPEED = 400                # above it, --clearance 1 or more is required: a servo overshoots its goal at speed
MAX_SPEED = 2000                # Goal_Speed ceiling this tool accepts (steps per second)
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, clearance: int = 0):
        self.guard, self.model, self.tol = guard, model, int(tolerance)
        self.clearance = int(clearance)
        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
        for _ in range(self.clearance):   # clearance: every grid point within that many steps of a forbidden one counts as forbidden
            grown = occ.copy()
            for k in range(occ.ndim):
                lo = tuple(slice(0, -1) if i == k else slice(None) for i in range(occ.ndim))
                hi = tuple(slice(1, None) if i == k else slice(None) for i in range(occ.ndim))
                grown[lo] |= occ[hi]
                grown[hi] |= occ[lo]
            occ = grown
        self.occ_used = 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, mode: str = "frontier") -> Plan:
        """mode "frontier": the tour hugs the forbidden region (the default);
        mode "random": a random walk over the allowed cells, any direction,
        unvisited cells first, the time to come home always kept."""
        budget = float(duration)
        rng = np.random.default_rng(seed)
        if mode == "random":
            return self._plan_random(budget, rng)
        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 _plan_random(self, budget: float, rng) -> Plan:
        visited = np.zeros(self.N, dtype=bool)
        cells, t, cur = [], 0.0, None
        ok_f = self.cell_ok.ravel()
        while True:
            if cur is None:
                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])
                ok = np.ones(len(nbr), dtype=bool)
            else:
                nbr = np.clip(cur + self.shift, 0, self.N - 1)
                costs, ok = self.cost, self.edge[:, cur]
            m = ok & ok_f[nbr] & (t + costs + self.dist[nbr] <= budget + 1e-9)
            if not m.any():
                break
            idx = np.flatnonzero(m)
            fresh = idx[~visited[nbr[idx]]]
            pool = fresh if len(fresh) else idx
            k = int(pool[int(rng.integers(len(pool)))])
            n = int(nbr[k])
            t += float(costs[k])
            visited[n] = True
            cells.append(n)
            cur = n
        if len(set(cells)) < 3:
            raise Refused(f"the random walk finds fewer than three allowed cells in reach of the drive pose "
                          f"in {budget:g} s 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, 0)
        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("--random", action="store_true", help="a random walk over the allowed cells instead of the boundary tour")
    ap.add_argument("--clearance", type=int, default=0, help="grid steps kept clear of any forbidden point (0); at least 1 above speed 400")
    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 a.speed > SLOW_SPEED and a.clearance < 1:
        say(f"refused: --speed {a.speed} above {SLOW_SPEED} needs --clearance 1 or more (overshoot at speed is not in the table)")
        return 1
    if a.clearance < 0:
        say("refused: --clearance below 0")
        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, a.clearance)
        plan = p.plan(a.duration, a.seed, a.spread, "random" if a.random else "frontier")
    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())