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
31.6 KB
Raw Blame
#!/usr/bin/env python3
"""The arm's pick-and-drop sequence, checked against the keep-out guard before it moves.

The sequence (config/arm_drop_poses.json) is a list of joint-space moves
between named poses: from the drive pose to a travel pose, down to the pick,
the gripper closing, up through lift to clear (the arm folded high over the
base), the base turned to the drop pose over the bin, the gripper opening,
back to clear, and from clear to the drive pose. Each move is a straight line
in joint space, timed by a quintic smootherstep (zero speed and acceleration
at both ends); the pan is never wrapped: the route turns the way the angles
say, never the short way round.

    tools/moss_arm_drop.py plan                       # the guard on every move, one line per move
    tools/moss_arm_drop.py plan --suggest             # also the smallest change that would pass, for each refusal
    tools/moss_arm_drop.py plan --as-delivered        # the poses as delivered, without the shifts and via poses
    tools/moss_arm_drop.py plan --mujoco auto         # also every sample posed in the CAD scene: contacts, tool point, shoulder load

Options
  --poses       the sequence (config/arm_drop_poses.json)
  --table       the keep-out table (config/arm_keepout.json)
  --calib       the arm's calibration (config/arm_calibration.json): the drive pose,
                the servo IDs and their ranges
  --clearance   grid steps kept clear of any forbidden grid point (1): the table's
                forbidden points grown by that many steps along each joint
  --tolerance   ticks: the box rule's margin around every sample (25)
  --speed       ticks per second: the fastest any joint may go (400); a move the
                sequence times faster is stretched; above 400 needs --clearance 1
  --as-delivered  the poses without their shift_deg and the moves without their via poses
  --suggest     for each refused pose, the smallest shift (whole degrees, one or
                two joints) that passes; for each refused move between two poses
                that pass, the via pose (a point of the move, shifted the same way)
                that splits it into two moves that pass
  --mujoco      a MOSS_R31.xml scene (auto: MOSS_ARM_CAD, else ~/moss-*/cad/*/MOSS_R31.xml)

What plan checks
  The guard's table (moss_dimos/arm_guard.py) with its forbidden grid points
  grown by --clearance. Every move is cut into samples, servo ticks rounded as
  they would be sent, no joint moving more than a quarter of a grid step from
  one sample to the next. Each sample: inside the calibrated range of every
  joint, and allowed by the guard's rule. Each pair of samples: the straight
  move between them allowed (segment_allowed), and the box rule of
  tools/moss_arm_sweep.py: every grid point of the box the pair spans, each
  end widened by --tolerance (the drive pose's end by nothing: it is the pose
  itself), is free. The first sample that fails names the reason and the
  rover part (the nearest forbidden grid point of the table itself when the
  refusal comes from the clearance). Exit status 0 when every move passes,
  1 when one is refused, 2 when the inputs do not load.

  The timing: each move keeps the sequence's duration unless a joint would go
  faster than --speed at the quintic's peak (1.875 times the mean speed):
  then it is stretched to that speed. The table knows poses, not overshoot:
  above 400 ticks per second the plan needs --clearance 1, as the sweep does.

  --mujoco poses every sample again (finer: one degree per joint at most) in
  the CAD scene, as tools/moss_arm_keepout.py builds it (the rover's parts,
  the arm with its fingers swept over every opening, the shoulder on its own,
  and the can of the table while the gripper holds it), and counts the
  contacts closer than the table's margin: zero expected. It also gives, for
  each pose, the tool point (in the model's world frame), its horizontal
  distance from the pan axis, and the shoulder's static load: the gravity
  torque of the links below the shoulder joint, from the model's masses
  (an order of magnitude: the model's masses are not weighed parts). It
  needs mujoco and runs at nice 10, one process.
"""
from __future__ import annotations

import argparse
import copy
import glob
import itertools
import json
import math
import os
import sys
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"))

import moss_arm_sweep as W  # noqa: E402  (the planner's clearance and box rule, the calibration loader)

POSES = ROOT / "config" / "arm_drop_poses.json"
FORMAT = "moss-arm-drop/1"
TICKS_PER_REV = 4096
SAMPLES_PER_STEP = 4            # a move is cut so that no joint moves more than 1/4 of a grid step between samples
QUINTIC_PEAK = 1.875            # the quintic smootherstep's peak speed over its mean speed
MJ_STEP_DEG = 1.0               # --mujoco: at most this many degrees per joint between two posed samples
SHIFT_MAX_DEG = 30              # --suggest: the largest shift tried, whole degrees
VIA_FRACTIONS = [0.5, 0.45, 0.55, 0.4, 0.6, 0.35, 0.65, 0.3, 0.7, 0.25, 0.75, 0.2, 0.8, 0.15, 0.85, 0.1, 0.9]
TCP_IN_TOOL_M = (0.0, 0.0, -0.035)   # the tool point, in the gripper's tool frame (35 mm along the approach)
TOOL_BODY = "nc90_tool"
LOAD_JOINT = "shoulder_lift"
PAN_JOINT = "shoulder_pan"
GRAVITY = 9.81


class Refused(Exception):
    """The inputs do not make a plan."""


def quintic(u: float) -> float:
    u = min(max(float(u), 0.0), 1.0)
    return u * u * u * (u * (u * 6.0 - 15.0) + 10.0)


def cubic(u: float) -> float:
    u = min(max(float(u), 0.0), 1.0)
    return u * u * (3.0 - 2.0 * u)


# ── the sequence ──

@dataclass
class Pose:
    name: str
    ticks: dict                 # as used: the shift applied unless --as-delivered
    delivered: dict             # ticks of the pose as delivered
    shift_deg: dict = field(default_factory=dict)
    added: bool = False         # a pose of ours (drive, a via pose), not of the delivered sequence


@dataclass
class Move:
    phase: str
    a: Pose
    b: Pose
    duration_s: float           # the sequence's own duration for this move
    gap_mm: tuple               # the jaw gap at the start and at the end
    holds_can: bool             # the gripper holds the can during this move
    added: bool = False


def gap_to_ticks(gripper: dict, gap_mm: float) -> int:
    """The gripper's goal for a jaw gap: the straight line through its open and grasp points."""
    o, g = gripper["open"], gripper["grasp"]
    k = (float(o["ticks"]) - float(g["ticks"])) / (float(o["gap_mm"]) - float(g["gap_mm"]))
    return int(round(float(g["ticks"]) + k * (float(gap_mm) - float(g["gap_mm"]))))


def load_doc(path) -> dict:
    try:
        doc = json.loads(Path(path).read_text())
    except (OSError, ValueError) as e:
        raise Refused(f"the sequence {path} does not load: {e}") from e
    if doc.get("format") != FORMAT:
        raise Refused(f"{path} is not a drop sequence (format {doc.get('format')!r}, want {FORMAT!r})")
    return doc


def build(doc: dict, jm, joints, drive: dict, as_delivered: bool = False) -> list:
    """The moves, in order, from the parsed sequence (angles in model radians,
    turned into ticks through jm, the table's JointMap): each via pose splits
    its move into two, the duration shared in proportion to each piece's
    largest joint move."""
    joints = list(joints)
    if list(doc.get("joints", [])) != joints:
        raise Refused(f"the sequence's joints {doc.get('joints')} are not the table's {joints}")

    def ticks(rad, shift):
        if len(rad) != len(joints):
            raise Refused(f"{len(rad)} angles for {len(joints)} joints")
        unknown = [j for j in shift if j not in joints]
        if unknown:
            raise Refused(f"shift_deg names {unknown}")
        return {j: jm.rad_to_ticks(j, float(v) + math.radians(float(shift.get(j, 0.0)))) for j, v in zip(joints, rad)}

    poses = {"drive": Pose("drive", dict(drive), dict(drive), added=True)}
    for name, p in doc["poses"].items():
        shift = {} if as_delivered else {j: float(v) for j, v in (p.get("shift_deg") or {}).items()}
        poses[name] = Pose(name, ticks(p["rad"], shift), ticks(p["rad"], {}), shift)
    moves = []
    for k, s in enumerate(doc["sequence"]):
        for key in ("from", "to"):
            if s[key] not in poses:
                raise Refused(f"move {k} ({s.get('phase')}): no pose {s[key]!r}")
        a, b = poses[s["from"]], poses[s["to"]]
        gap = tuple(float(v) for v in s.get("gap_mm", (None, None)))
        pts = [a]
        for i, v in enumerate([] if as_delivered else (s.get("via") or [])):
            t = ticks(v["rad"], {})
            pts.append(Pose(f"via {a.name}>{b.name}" + (f" {i + 1}" if i else ""), t, t, added=True))
        pts.append(b)
        spans = [max(abs(q.ticks[j] - p.ticks[j]) for j in joints) for p, q in zip(pts, pts[1:])]
        total = sum(spans)
        for p, q, d in zip(pts, pts[1:], spans):
            share = d / total if total else 1.0 / len(spans)
            moves.append(Move(s["phase"], p, q, float(s["duration_s"]) * share, gap,
                              bool(s.get("holds_can", False)), bool(s.get("added", False))))
    return moves


def samples(a: dict, b: dict, joints, step: dict, per_step: int = SAMPLES_PER_STEP) -> list:
    """The move a -> b cut so that no joint moves more than step[j] / per_step
    ticks between two samples, rounded to ticks (half up, in exact integers:
    samples(b, a) is samples(a, b) reversed); a and b included."""
    most = {j: max(1, int(step[j] // per_step)) for j in joints}     # ticks: the rounding never steps past it
    n = max(1, max(int(math.ceil(abs(b[j] - a[j]) / most[j])) for j in joints))
    return [{j: (2 * (int(a[j]) * n + (int(b[j]) - int(a[j])) * k) + n) // (2 * n) for j in joints} for k in range(n + 1)]


def peak_speed(m: Move, joints) -> float:
    """Ticks per second at the quintic's peak, on the joint that moves most."""
    d = max(abs(m.b.ticks[j] - m.a.ticks[j]) for j in joints)
    return QUINTIC_PEAK * d / m.duration_s if m.duration_s > 0 else (math.inf if d else 0.0)


def stretched(m: Move, joints, speed: float) -> float:
    d = max(abs(m.b.ticks[j] - m.a.ticks[j]) for j in joints)
    return max(m.duration_s, QUINTIC_PEAK * d / speed) if d else m.duration_s


# ── the checks ──

class Checker:
    """The guard's rule, segment_allowed and the sweep's box rule, on the
    table's forbidden grid points grown by `clearance` grid steps."""

    def __init__(self, guard, calibration: dict, tolerance: int = 25, clearance: int = 1):
        self.guard, self.tol, self.clearance = guard, int(tolerance), int(clearance)
        model = W.SpeedModel(W.SLOW_SPEED, 8, self.tol)
        grown = W.Planner(guard, calibration, model, self.tol, self.clearance)   # the sweep's clearance
        self.used = copy.copy(guard)
        self.used.occ = grown.occ_used
        self.used._targets = None
        self.p = W.Planner(self.used, calibration, model, self.tol, 0)           # its box rule on that table
        self.joints = list(guard.joints)
        self.drive = dict(self.p.drive)
        self.ranges = dict(self.p.ranges)
        self.step = {j: s for j, s in zip(self.joints, self.p.step_ticks)}

    def rad(self, t: dict) -> dict:
        return self.p.rad(t)

    def why(self, t: dict) -> str:
        """Why a pose is refused: the table's own why() where the table refuses
        it, else the forbidden grid point of the table the clearance grew from."""
        q = self.rad(t)
        if not self.guard.allowed(q):
            return self.guard.why(q)
        f = self.guard._fracs(q)
        if self.used._outside(f) is not None:
            return self.used.why(q)
        return self._grown_why(self.guard._axes(f), f)

    def _grown_why(self, axes, f) -> str:
        c = self.clearance
        idx = [np.arange(max(min(a) - c, 0), min(max(a) + c, n - 1) + 1) for a, n in zip(axes, self.guard.shape)]
        sub = self.guard.occ[np.ix_(*idx)]
        pts = np.argwhere(sub) + np.array([i[0] for i in idx])
        if not len(pts):
            return "refused (no forbidden grid point of the table nearby: check the clearance)"
        d = np.abs(pts - f).max(axis=1)
        p = tuple(int(x) for x in pts[int(np.argmin(d))])
        q = {j: float(g[p[k]]) for k, (j, g) in enumerate(zip(self.joints, self.guard.grids))}
        return (f"within {c} grid step(s) of a forbidden grid point (clearance {c}): "
                + self.guard.why(q).replace("forbidden: ", ""))

    def in_range(self, t: dict) -> str | None:
        out = [f"{j} {t[j]} outside {lo}..{hi}" for j, (lo, hi) in self.ranges.items() if not lo <= t[j] <= hi]
        return "; ".join(out) or None

    def robust(self, t: dict) -> bool:
        """The pose and its box widened by the tolerance pass (a pose to stop at)."""
        return self.in_range(t) is None and self.used.allowed(self.rad(t)) and self.p.box_free(t, t)

    def pair(self, a: dict, b: dict) -> str | None:
        """None when the step a -> b passes every check, else why not."""
        for t in (a, b):
            r = self.in_range(t)
            if r:
                return f"outside the calibrated range: {r}"
            if not self.used.allowed(self.rad(t)):
                return f"the pose is refused: {self.why(t)}"
        if not self.used.segment_allowed(self.rad(a), self.rad(b)):
            return f"segment_allowed refuses the step: {self.why(b)}"
        if not self.p.box_free(a, b):
            return f"the box rule refuses the step (a forbidden grid point within {self.tol} ticks): {self._box_why(a, b)}"
        return None

    def _box_why(self, a: dict, b: dict) -> str:
        qa, qb = self.rad(a), self.rad(b)
        pad = self.tol * 2.0 * math.pi / TICKS_PER_REV
        pa = 0.0 if a == self.drive else pad
        pb = 0.0 if b == self.drive else pad
        idx = []
        for j, g in zip(self.joints, self.guard.grids):
            lo, hi = min(qa[j] - pa, qb[j] - pb), max(qa[j] + pa, qb[j] + pb)
            tl, th = W._touched(W._frac(g, lo), len(g)), W._touched(W._frac(g, hi), len(g))
            if tl is None or th is None:
                return f"{j} leaves the table"
            idx.append(np.arange(tl[0], th[1] + 1))
        sub = self.used.occ[np.ix_(*idx)]
        pts = np.argwhere(sub) + np.array([i[0] for i in idx])
        if not len(pts):
            return "nothing found"
        f = np.array([W._frac(g, (qa[j] + qb[j]) / 2.0) for j, g in zip(self.joints, self.guard.grids)])
        p = tuple(int(x) for x in pts[int(np.argmin(np.abs(pts - f).max(axis=1)))])
        q = {j: float(g[p[k]]) for k, (j, g) in enumerate(zip(self.joints, self.guard.grids))}
        if self.guard.occ[p]:
            return self.guard.why(q).replace("forbidden: ", "")
        return self._grown_why([(i,) for i in p], np.array(p, dtype=float))

    def move(self, a: dict, b: dict) -> tuple[int, str | None, float | None]:
        """(samples, why the first failing step fails or None, where along the move 0..1)."""
        pts = samples(a, b, self.joints, self.step)
        n = len(pts) - 1
        for k in range(n):
            why = self.pair(pts[k], pts[k + 1])
            if why:
                return n + 1, why, (k + 1) / n if n else 1.0
        if n == 0:
            why = self.pair(pts[0], pts[0])
            if why:
                return 1, why, 0.0
        return n + 1, None, None


# ── the smallest change ──

def _shifts(max_deg: int):
    """(joint indices, degrees) for one or two joints, by the largest shift
    first, then the sum, whole degrees up to max_deg."""
    out = []
    vals = list(range(-max_deg, max_deg + 1))
    for k in (1, 2):
        for js in itertools.combinations(range(5), k):
            for combo in itertools.product(vals, repeat=k):
                if 0 in combo:
                    continue
                out.append((max(map(abs, combo)), sum(map(abs, combo)), k, js, combo))
    out.sort(key=lambda x: x[:3])
    return out


def _shifted(c: Checker, t: dict, js, combo) -> dict:
    """t with joints js turned by combo degrees (through the table's JointMap)."""
    out = dict(t)
    jm = c.guard.joint_map
    for i, d in zip(js, combo):
        j = c.joints[i]
        out[j] = jm.rad_to_ticks(j, jm.ticks_to_rad(j, t[j]) + math.radians(d))
    return out


def suggest_pose(c: Checker, t: dict, max_deg: int = SHIFT_MAX_DEG):
    """The smallest shift ({joint: degrees}) after which the pose passes with
    its tolerance box, or None."""
    for m, _, _, js, combo in _shifts(max_deg):
        if c.robust(_shifted(c, t, js, combo)):
            return {c.joints[i]: d for i, d in zip(js, combo)}
    return None


def suggest_via(c: Checker, a: dict, b: dict, max_deg: int = SHIFT_MAX_DEG):
    """A via pose (ticks) on the move a -> b, shifted by the smallest whole
    degrees on one or two joints, that splits it into two moves that pass;
    (via ticks, fraction, shift) or None."""
    shifts = [(0, 0, 0, (), ())] + _shifts(max_deg)
    for m in range(0, max_deg + 1):
        level = [s for s in shifts if s[0] == m]
        for f in VIA_FRACTIONS:
            base = {j: int(round(a[j] + (b[j] - a[j]) * f)) for j in c.joints}
            for _, _, _, js, combo in level:
                v = _shifted(c, base, js, combo)
                if not c.robust(v):
                    continue
                if not (c.used.segment_allowed(c.rad(a), c.rad(v)) and c.used.segment_allowed(c.rad(v), c.rad(b))):
                    continue
                if c.move(a, v)[1] is None and c.move(v, b)[1] is None:
                    return v, f, {c.joints[i]: d for i, d in zip(js, combo)}
    return None


# ── the plan ──

@dataclass
class Verdict:
    move: Move
    samples: int
    why: str | None
    at: float | None
    duration_s: float
    peak: float


def check_moves(c: Checker, moves: list, speed: float) -> list:
    out = []
    for m in moves:
        n, why, at = c.move(m.a.ticks, m.b.ticks)
        out.append(Verdict(m, n, why, at, stretched(m, c.joints, speed), peak_speed(m, c.joints)))
    return out


def _deg(t: dict, c: Checker) -> str:
    return " ".join(f"{math.degrees(v):+7.1f}" for v in c.rad(t).values())


def print_plan(c: Checker, verdicts: list, doc: dict, speed: float, say=print) -> None:
    say(f"table: {c.guard.summary()}")
    say(f"checks: clearance {c.clearance} grid step(s) (grid steps "
        + ", ".join(f"{j} {s:.0f}" for j, s in c.step.items()) + " ticks), "
        f"box rule tolerance {c.tol} ticks, samples at most a quarter of a grid step apart")
    say("poses (ticks; model degrees " + " ".join(j[:8] for j in c.joints) + "):")
    seen = {}
    for v in verdicts:
        for p in (v.move.a, v.move.b):
            seen.setdefault(p.name, p)
    for p in seen.values():
        sh = (" shifted " + ", ".join(f"{j} {d:+g} deg" for j, d in p.shift_deg.items())
              + f" (delivered {[p.delivered[j] for j in c.joints]})") if p.shift_deg else ""
        say(f"  {p.name:16s} {str([p.ticks[j] for j in c.joints]):29s} {_deg(p.ticks, c)}{sh}")
    g = doc.get("gripper") or {}
    if g:
        say(f"gripper: open {g['open']['gap_mm']:g} mm = {g['open']['ticks']} ticks, grasp {g['grasp']['gap_mm']:g} mm "
            f"= {g['grasp']['ticks']} ticks (force {g.get('force')} at registers {g.get('force_registers')})")
    say(f"{'move':40s} {'samples':>7s} {'seq s':>6s} {'peak t/s':>8s} {'run s':>6s}  verdict")
    t_seq = t_run = 0.0
    for v in verdicts:
        m = v.move
        name = f"{m.phase}: {m.a.name} -> {m.b.name}" if m.a is not m.b and m.a.name != m.b.name else f"{m.phase}: at {m.a.name}"
        if m.gap_mm[0] != m.gap_mm[1] and g:
            name += f" (gap {m.gap_mm[0]:g} -> {m.gap_mm[1]:g} mm)"
        verdict = "OK" if v.why is None else f"REFUSED at {v.at:.2f}: {v.why}"
        say(f"{name[:40]:40s} {v.samples:7d} {m.duration_s:6.2f} {v.peak:8.0f} {v.duration_s:6.2f}  {verdict}")
        t_seq += m.duration_s
        t_run += v.duration_s
    bad = [v for v in verdicts if v.why]
    say(f"sequence: {len(verdicts)} moves, {t_seq:.1f} s as timed, {t_run:.1f} s with no joint over {speed:g} ticks/s; "
        + ("every move passes" if not bad else f"{len(bad)} move(s) refused"))


def print_suggestions(c: Checker, verdicts: list, say=print) -> dict:
    """For each refused pose its smallest shift, then for each refused move
    between poses that pass (shifts applied) a via pose."""
    out = {"shift_deg": {}, "via": {}}
    names = {}
    for v in verdicts:
        for p in (v.move.a, v.move.b):
            names.setdefault(p.name, p)
    fixed = {}
    for name, p in names.items():
        if c.robust(p.ticks):
            fixed[name] = p.ticks
            continue
        s = suggest_pose(c, p.ticks)
        if s is None:
            say(f"suggest: pose {name}: no shift of one or two joints up to {SHIFT_MAX_DEG} deg passes ({c.why(p.ticks)})")
            continue
        fixed[name] = _shifted(c, p.ticks, [c.joints.index(j) for j in s], list(s.values()))
        total = {j: p.shift_deg.get(j, 0.0) + d for j, d in s.items()}
        out["shift_deg"][name] = total
        say(f"suggest: pose {name}: shift " + ", ".join(f"{j} {d:+d} deg" for j, d in s.items())
            + f" -> {[fixed[name][j] for j in c.joints]} ({c.why(p.ticks)})")
    for v in verdicts:
        a, b = v.move.a.name, v.move.b.name
        if a not in fixed or b not in fixed or a == b:
            continue
        ta, tb = fixed[a], fixed[b]
        if c.move(ta, tb)[1] is None:
            continue
        r = suggest_via(c, ta, tb)
        if r is None:
            say(f"suggest: move {a} -> {b}: no via pose found (shifts up to {SHIFT_MAX_DEG} deg)")
            continue
        via, f, s = r
        out["via"][f"{a}->{b}"] = {"ticks": [via[j] for j in c.joints], "at": f, "shift_deg": s}
        say(f"suggest: move {a} -> {b}: a via pose at {f:.2f} of the move"
            + (", shifted " + ", ".join(f"{j} {d:+d} deg" for j, d in s.items()) if s else "")
            + f": {[via[j] for j in c.joints]} ({_deg(via, c)} deg)")
    return out


# ── the CAD scene ──

def find_cad(arg: str) -> Path | None:
    if arg and arg != "auto":
        return Path(arg)
    env = os.environ.get("MOSS_ARM_CAD", "").strip()
    if env:
        return Path(env)
    found = sorted(glob.glob(os.path.expanduser("~/moss-*/cad/*/MOSS_R31.xml")))
    return Path(found[-1]) if found else None


@dataclass
class SceneReport:
    contacts: list              # per move: {"arm": n, "shoulder": n, "can": n, "parts": {name: n}, "samples": n, "errors": n}
    poses: dict                 # name: {"tcp_m", "axis_m", "height_m", "shoulder_nm", "lever_m"}
    margin_m: float
    model: str


def scene_check(c: Checker, moves: list, model_path: Path, say=print) -> SceneReport:
    """Every move posed in the CAD scene, one degree per joint at most between
    samples; contacts closer than the table's margin between the rover's parts
    and the arm (fingers swept), the shoulder (as the table counts it: every
    contact), and the table's can while the gripper holds it."""
    import gc

    import mujoco
    import moss_arm_keepout as K

    if os.nice(0) < 10:
        os.nice(10 - os.nice(0))
    K._lean_malloc()
    layout = K.layout_of(model_path)
    L = K.LAYOUTS[layout]
    margin = float(c.guard.meta.get("margin_m") or L["margin"])
    pay = (c.guard.meta.get("payload") or {})
    payload = K.parse_payload(pay.get("kind", "none")) if pay else None
    if payload and pay.get("diameter_m"):
        payload = dict(payload, diameter_m=float(pay["diameter_m"]), height_m=float(pay["height_m"]))
    manifest = model_path.parent / "parts_manifest.json"
    names = K.load_names(manifest) if manifest.is_file() else {}
    p = K.prepare(model_path, arm_root=L["arm_root"], rover_body=L["rover_body"],
                  rover_parts=tuple(s.strip() for s in L["rover_parts"].split(",") if s.strip()),
                  margin=margin, lidar_xyz=None, joints=c.joints, visual=L["visual"],
                  root_voxel=L["root_voxel"], names=names, payload=payload)
    model = p.model
    jid = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, j) for j in c.joints]
    adr = [int(model.jnt_qposadr[j]) for j in jid]
    is_arm, is_part = K._masks(model, p.arm_ids, p.part_ids)
    is_root, _ = K._masks(model, p.root_ids, p.part_ids)
    pay_ids = list(p.payload_ids.values()) if p.payload_model is not None else []
    is_pay, _ = K._masks(model, pay_ids, p.part_ids) if pay_ids else (None, None)
    part_name = {g: p.display[k] for g, k in zip(p.part_ids, p.part_label)}
    d_arm, d_root = mujoco.MjData(model), mujoco.MjData(p.root_model)
    d_pay = mujoco.MjData(p.payload_model) if pay_ids else None

    def pose(data, base, t):
        data.qpos[:] = base
        for a, j in zip(adr, c.joints):
            data.qpos[a] = c.guard.joint_map.ticks_to_rad(j, t[j])

    step = {j: MJ_STEP_DEG * TICKS_PER_REV / 360.0 for j in c.joints}
    report = []
    for m in moves:
        r = {"arm": 0, "shoulder": 0, "can": 0, "parts": {}, "samples": 0, "errors": 0}
        for t in samples(m.a.ticks, m.b.ticks, c.joints, step, 1):
            r["samples"] += 1
            pose(d_arm, p.base_qpos, t)
            hits = {"arm": K._touch(model, d_arm, is_arm, is_part, margin)}
            pose(d_root, p.base_qpos, t)
            hits["shoulder"] = K._touch(p.root_model, d_root, is_root, is_part, math.inf)
            if m.holds_can and d_pay is not None:
                pose(d_pay, p.payload_qpos, t)
                hits["can"] = K._touch(p.payload_model, d_pay, is_pay, is_part, margin)
            for k, h in hits.items():
                if h is None:
                    r["errors"] += 1
                    continue
                r[k] += len(h)
                for _, g, _ in h:
                    r["parts"][part_name.get(g, str(g))] = r["parts"].get(part_name.get(g, str(g)), 0) + 1
        report.append(r)
    # the poses: tool point, distance from the pan axis, the shoulder's static load
    tool = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, TOOL_BODY)
    pan = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, PAN_JOINT)
    lift = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, LOAD_JOINT)
    below = [b for b in range(model.nbody) if _under(model, b, int(model.jnt_bodyid[lift]))]
    poses = {}
    for m in moves:
        for q in (m.a, m.b):
            if q.name in poses:
                continue
            pose(d_arm, p.base_qpos, q.ticks)
            mujoco.mj_kinematics(model, d_arm)
            tcp = d_arm.xpos[tool] + d_arm.xmat[tool].reshape(3, 3) @ np.array(TCP_IN_TOOL_M)
            ax = d_arm.xanchor[pan]
            g = np.array([0.0, 0.0, -GRAVITY])
            tau, mass, mom = 0.0, 0.0, np.zeros(3)
            for b in below:
                tau += float(d_arm.xaxis[lift] @ np.cross(d_arm.xipos[b] - d_arm.xanchor[lift], model.body_mass[b] * g))
                mass += float(model.body_mass[b])
                mom += model.body_mass[b] * d_arm.xipos[b]
            v = mom / mass - d_arm.xanchor[lift]
            v -= (v @ d_arm.xaxis[lift]) * d_arm.xaxis[lift]
            poses[q.name] = {"tcp_m": [round(float(x), 4) for x in tcp],
                             "axis_m": round(float(np.hypot(*(tcp - ax)[:2])), 4),
                             "height_m": round(float(tcp[2] - ax[2]), 4),
                             "shoulder_nm": round(tau, 3), "lever_m": round(float(np.hypot(v[0], v[1])), 4),
                             "mass_kg": round(mass, 3)}
    del d_arm, d_root, d_pay, model, p
    gc.collect()
    K._trim()
    return SceneReport(report, poses, margin, model_path.name)


def _under(model, b: int, top: int) -> bool:
    while b > 0:
        if b == top:
            return True
        b = int(model.body_parentid[b])
    return False


def print_scene(s: SceneReport, verdicts: list, say=print) -> None:
    say(f"CAD scene {s.model}: contacts closer than {s.margin_m * 1000:g} mm (the arm with its fingers swept, "
        "the shoulder, the can while held)")
    for v, r in zip(verdicts, s.contacts):
        m = v.move
        name = f"{m.phase}: {m.a.name} -> {m.b.name}" if m.a.name != m.b.name else f"{m.phase}: at {m.a.name}"
        parts = ", ".join(f"{k} x{n}" for k, n in sorted(r["parts"].items(), key=lambda kv: -kv[1])[:3])
        say(f"  {name[:40]:40s} {r['samples']:4d} poses: arm {r['arm']}, shoulder {r['shoulder']}, "
            f"can {r['can'] if m.holds_can else '-'}" + (f", errors {r['errors']}" if r["errors"] else "")
            + (f" ({parts})" if parts else ""))
    total = sum(r["arm"] + r["shoulder"] + r["can"] + r["errors"] for r in s.contacts)
    say(f"  total contacts: {total}")
    say("poses: the tool point in the model's world frame, its distance from the pan axis, its height over the "
        "pan axis's anchor, the shoulder's static load (links below the shoulder joint, the model's masses)")
    for name, p in s.poses.items():
        say(f"  {name:16s} tool {p['tcp_m']} m, {p['axis_m'] * 100:5.1f} cm from the axis, {p['height_m'] * 100:+5.1f} cm; "
            f"shoulder {p['shoulder_nm']:+.2f} N m ({p['mass_kg']:g} kg, centre of mass {p['lever_m'] * 100:.1f} cm out)")


# ── the command line ──

def plan_main(a, say=print) -> int:
    if a.speed <= 0 or a.speed > W.MAX_SPEED:
        say(f"refused: --speed {a.speed:g}: above 0, at most {W.MAX_SPEED}")
        return 2
    if a.clearance < 0:
        say("refused: --clearance below 0")
        return 2
    try:
        guard, calibration = W.load(a.table, a.calib)
        doc = load_doc(a.poses)
        c = Checker(guard, calibration, a.tolerance, a.clearance)
        moves = build(doc, guard.joint_map, c.joints, c.drive, a.as_delivered)
    except (W.Refused, Refused, KeyError, ValueError) as e:
        say(f"refused: {e}")
        return 2
    verdicts = check_moves(c, moves, a.speed)
    print_plan(c, verdicts, doc, a.speed, say)
    fast = max((v.peak for v in verdicts), default=0.0)
    code = 1 if any(v.why for v in verdicts) else 0
    if a.speed > W.SLOW_SPEED and a.clearance < 1:
        say(f"refused: --speed {a.speed:g} above {W.SLOW_SPEED} needs --clearance 1 or more (overshoot at speed is not in the table)")
        code = 1
    if fast > W.SLOW_SPEED:
        say(f"note: the sequence's own timing peaks at {fast:.0f} ticks/s; the run times above stretch it to {a.speed:g}")
    if a.suggest and code:
        print_suggestions(c, verdicts, say)
    if a.mujoco:
        path = find_cad(a.mujoco)
        if path is None or not path.is_file():
            say(f"refused: --mujoco: no CAD scene ({a.mujoco}; MOSS_ARM_CAD, or ~/moss-*/cad/*/MOSS_R31.xml)")
            return 2
        try:
            import mujoco  # noqa: F401
        except ImportError:
            say("refused: --mujoco needs mujoco (pip install mujoco)")
            return 2
        s = scene_check(c, moves, path, say)
        print_scene(s, verdicts, say)
        if any(r["arm"] + r["shoulder"] + r["can"] + r["errors"] for r in s.contacts):
            code = 1
    return code


def main(argv=None, say=print) -> int:
    ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
    sub = ap.add_subparsers(dest="command", required=True)
    pp = sub.add_parser("plan", help="check the sequence against the guard (nothing moves)")
    pp.add_argument("--poses", default=str(POSES))
    pp.add_argument("--table", default=str(W.TABLE))
    pp.add_argument("--calib", default=str(W.CALIBRATION))
    pp.add_argument("--clearance", type=int, default=1)
    pp.add_argument("--tolerance", type=int, default=25)
    pp.add_argument("--speed", type=float, default=float(W.SLOW_SPEED))
    pp.add_argument("--as-delivered", action="store_true")
    pp.add_argument("--suggest", action="store_true")
    pp.add_argument("--mujoco", default="", help="a MOSS_R31.xml, or auto")
    a = ap.parse_args(argv)
    if a.command == "plan":
        return plan_main(a, say)
    return 2


if __name__ == "__main__":
    sys.exit(main())