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
117 KB
Raw Blame
#!/usr/bin/env python3
"""The arm's keep-out table: every arm pose that touches the rover, swept on a
MuJoCo model of the robot.

Two model layouts, told apart by their body names (--layout auto):
  * cad: the CAD scene (a body "rover_exterior"). Every part of the CAD has
    its own collision geoms (one convex hull, or the convex pieces of a
    decomposition, named <part>_NNN) and its visual meshes (contype and
    conaffinity 0). Defaults: --rover-body rover_exterior,base (the rover's
    exterior and the arm's own base, which hangs from the world, not from
    the rover), --arm-root shoulder, --rover-parts "*" (every collision geom
    counts as it is), --visual skip (the visual meshes are left out, on the
    arm and on the rover: the collision geoms stand for them; a body with no
    collision geom keeps its visual meshes, nothing else stands for them:
    the gripper's drive gear and servo horn), --root-voxel
    0 (the root link's collision geoms count as they are), no lidar added
    (the scene has the lidar, its base and its carriage as parts), --margin
    0.002 (the hulls are the parts themselves, and the folded arm rests 3 to
    4 mm from the bin's front wall: 10 mm would forbid the driving pose).
  * sim: the simulation model (a body "rover", or anything else). Defaults:
    --rover-body rover, --arm-root shoulder_link, --rover-parts hull,bin_*,
    track_*,tread_*,lidar, --visual count (a visual mesh is a part too),
    --root-voxel 0.001, the lidar cylinder added at the profile's pose,
    --margin 0.01.
Any of these flags given on the command line wins over the layout.

The arm is every geom from --arm-root down (with --visual count the visual
meshes too). At every point of a grid over its five joints the model is posed
(mj_kinematics, the position stage of mj_forward) and mj_collision lists the
contacts closer than --margin between an arm geom and a rover part. A grid
point with one such contact is forbidden. Nothing is stepped: no dynamics,
the arm is placed, never moved. A mesh collides as its convex hull, the whole
hull: the tool lifts the model's cap on hull vertices (maxhullvert), which
would cut a hull to fewer vertices and leave the part's corners outside it.

The root link (the body --arm-root, which only its own joints move: the pan)
is done apart. It is placed every --root-step-deg along its own joints, and a
contact there forbids every grid value within half a grid step (a pose
between two grid values touches both: the guard's rule), whatever the other
joints do. It turns in place at a clearance the build sets, so it counts
contacts closer than --root-margin (0: where it meets the rover), not
--margin. With --root-voxel above 0 its meshes are replaced by stand-ins that
follow their surface (below): the convex hull of a bracket fills its inside
and would sweep through whatever sits next to the arm's base.

The rover is every geom of the bodies --rover-body lists (comma separated)
and of the bodies under them, except the arm, the bodies between the arm and
the nearest listed body above it (its mount: base_link in the simulation
model), the visual meshes with --visual skip, and the geoms --exclude names:
  * the geoms --rover-parts names and every other primitive (box, sphere,
    capsule, cylinder, ellipsoid) count as they are;
  * every other mesh counts through stand-ins: its convex hull would fill a
    concave part (the inside of the bin, the deck around the arm's base).
    Its surface is sampled every half voxel, the samples more than
    --cover-tol outside the parts above are kept, and boxes on a
    --stand-in-voxel grid cover them (whole voxels, merged). Every point of
    the surface lies inside a stand-in or within --cover-tol plus half a
    voxel of a part that counts as it is; a stand-in reaches past the surface
    by less than one and a half voxels along each axis. The root link's
    stand-ins use --root-voxel and cover all of its surface.
The JSON lists every rover mesh that has stand-ins with the share of its
surface the parts that count as they are miss, where that share lies, and
its number of boxes. A rover geom held whole inside another that counts as
it is (as MuJoCo collides it: a mesh's hull, a primitive) adds no forbidden
pose, since an arm geom near it is nearer the other: it is left out and
listed (rover_inside; --keep-inside keeps it). In the CAD scene that drops
the wheels inside the track envelopes and the hardware inside the hull's
envelope.

The rover's geoms are counted: those that count as they are, the meshes
with stand-ins, those held inside another, and those left out by name add
up to every collision geom of the rover bodies (rover_geoms_counted,
rover_inside, rover_left_out in the JSON).

Parts. The collision geoms of one CAD part count as one part: a geom named
<part>_NNN (three digits) or <part>_envelope, after a part name that ends in
digits, is a piece of <part> (col_part_0996_017 -> col_part_0996). --names
<parts_manifest.json> ({"parts": [{"id": "part_0996", "name": ...}]}) gives
each part its name from the CAD: the part col_part_0996 (or part_0996) shows
as that name in the counts, the summary and the guard's why(). A name the
CAD exporter made up ("Open CASCADE STEP translator ...") takes the name of
the component that holds it, a name several parts share gets the part's id
in brackets, and the CAD's working code for the gripper (the first word of
its parts' names, two capitals and two digits) reads "gripper". A visual
mesh that counts (above) is named after the parts of its body in its colour.
Without --names the part key is the name.

The model's own exclusions between an arm body and a rover body are kept
(the CAD scene excludes the base and the shoulder: one turns on the other);
its other exclusions and its explicit pairs are dropped, and the parent
filter is off: the masks below decide every other contact. A rover part that
the root link touches at every value of its joints is where the arm sits:
the root link's contacts with it are not counted (every other link's are).
Such parts are printed and listed in the JSON (mount_parts).

The lidar. In the simulation model it is not there: when no geom is named
"lidar", a cylinder (radius 0.038 m, height 0.04 m) is added on the first
rover body at the profile's pose: x = y = 0 (base_link is the lidar's floor
projection, on the rotation centre) and z = lidar_height_m
(moss_dimos/robot_profile.py, MOSS_LIDAR_HEIGHT_M). --lidar-xyz moves it,
--no-lidar leaves it out. The rover parts it overlaps are printed and stored
(lidar.overlaps_m). In the CAD scene the lidar is a part like the others and
nothing is added unless --lidar-xyz asks for it.

The fingers (every arm joint that is not swept) take every position: each
geom they carry is replaced by its swept volume, the convex hull of the geom
at both ends of a slide (every 10 degrees of a hinge), on the body above the
finger. A closed, open or half-open gripper is inside it. --fix name=value
pins a joint to one value instead. The base pose of the sweep holds the
fingers open: each finger joint at the end of its range that spreads the
gripper the most, and a joint that one of the model's joint equalities ties
to another finger joint follows it (the CAD scene's right jaw follows the
left one, its pinion follows the left jaw's rack: MuJoCo applies none of
that under mj_kinematics, so the tool sets them itself). The JSON says which
(fingers.open).

The margin is MuJoCo's geom margin on the rover parts only (MuJoCo adds the
margins of the two geoms of a pair). MuJoCo's own convex collider is switched
off for libccd's: the former (3.9) reports no contact for a geom held whole
inside a mesh's hull. libccd says whether two geoms come within the margin;
the distance it reports with a contact is approximate (the verdicts use
only the first). The arena is raised to 256 MB per MjData (reserved, used
only by a pose deep in the rover, which makes 10000 contacts and more). A
pose where MuJoCo's collision code still fails (mujoco.FatalError) is marked
forbidden and counted (collision_errors).

Each joint is swept over its model range widened by --pad-deg on both sides:
the real arm's end stops (its calibrated travel) sit a few degrees past the
SO-101 model's limits, and the folded driving pose lies on them. With
--calibration (the arm's config/arm_calibration.json) the grid also covers
each joint's calibrated travel, range_min to range_max through the
joint_map, plus 3 degrees past each end (travel in the JSON): the guard
refuses a pose outside the table, so the table must hold every pose the
arm can reach.

The payload (--payload; the CAD layout: can, the simulation layout: none).
A pose the table allows must stay clear with a can in the gripper. The can
is a cylinder (66 mm across, 120 mm tall; cyl:<d>x<h> gives another) added
to the fingers' body as an arm geom (it meets the rover parts only), the
fingers closed on it: their pads, the parts of the two fingers past the
hand's own geoms, read from the finger geoms at the slide values, are its
diameter apart. It is centred between the pads' inner faces and across
them, its centre at the pads' middle along the approach (the hand's axis,
perpendicular to the slide, that points from the hand to the fingers),
then pushed out along the approach until it clears the hand's other geoms:
a can cannot go into the palm. It is held two ways: upright, its axis along
the approach (a can standing on the floor, picked from above), and across,
its axis across both the slide and the approach (a can lying down, picked
from the side). The table is the union of the empty gripper (every jaw
position) and the can held either way: the can is swept alone, the arm at
the holding gap lying inside the jaws' swept volumes. The JSON records the
payload: its size, the finger values that hold it, the pads, its pose in
the fingers' body's frame for each orientation, and the forbidden cells of
each sweep (payload, occupancy_empty_bits_hex). --reuse-empty takes the
empty gripper's sweep from an earlier table of the same model (both
sha256), grid and settings, and sweeps only the payload.

Checks before writing: --check grid points are done again with a full
mj_forward and must agree (the empty gripper's own cells, then the table
with the payload), and --check free grid points are done again with
the real fingers closed, half open, open and at a random opening: none may
touch the rover.

    python tools/moss_arm_keepout.py --model MOSS_R31.xml --names parts_manifest.json \
        --calibration config/arm_calibration.json                                       # -> config/arm_keepout.json
    python tools/moss_arm_keepout.py --model <robot.xml> --steps 8,8,8,6,4 --out /tmp/k.json

Options: --layout (auto, cad, sim), --steps pan,lift,elbow,flex,roll
(16,16,16,12,8), --margin metres, --pad-deg (10), --calibration (none),
--joints (the five SO-101 joint names), --arm-root, --rover-body,
--rover-parts, --visual (count, skip), --names, --exclude (none),
--keep-inside, --stand-in-voxel (0.004 m, 0 = rover meshes left out),
--root-voxel (0 = the root link's geoms as they are), --root-margin (0 m),
--root-step-deg (1), --cover-tol (0.002 m), --lidar-xyz x,y,z, --no-lidar,
--fix, --payload (by layout), --reuse-empty, --check N (64), --jobs (one
per core, up to 16), --quiet.

The table lists only the parts the sweep hit (part_names); cell_part names
each forbidden cell's part by its number in that list, one byte per cell
while the list holds 255 parts or fewer, two bytes beyond (cell_part_dtype).
pair_counts names the most frequent (arm part, rover part) pairs, pair_cells
holds them all by number.
The JSON keeps the joint_map (ticks <-> model radians) of the file it
replaces, with its basis: the alignment measured on the arm survives a new
sweep. It names the model by its file name, never its path (the two sha256
say which file). Needs mujoco and numpy here (scipy, when present, lets a
mesh named in --rover-parts cover the stand-ins with its hull);
moss_dimos/arm_guard.py reads the table with numpy only. docs/arm.md has
the whole story.
"""
from __future__ import annotations

import argparse
import fnmatch
import hashlib
import itertools
import json
import math
import os
import re
import sys
import time
from collections import Counter
from pathlib import Path

import numpy as np

ROOT = Path(__file__).resolve().parent.parent
sys.path.insert(0, str(ROOT))

FORMAT = "moss-arm-keepout/1"
DEFAULT_OUT = ROOT / "config" / "arm_keepout.json"
JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll")
STEPS = (16, 16, 16, 12, 8)
ROVER_PARTS = ("hull", "bin_*", "track_*", "tread_*", "lidar")
MARGIN_M = 0.01
CAD_MARGIN_M = 0.002
PAD_DEG = 10.0
VOXEL_M = 0.004
ROOT_VOXEL_M = 0.001
ROOT_MARGIN_M = 0.0
ROOT_STEP_DEG = 1.0
COVER_TOL_M = 0.002
LIDAR_NAME = "lidar"
LIDAR_RADIUS_M = 0.038
LIDAR_HEIGHT_M = 0.04
TICKS_PER_REV = 4096
MODEL_ZERO_TICKS = 2048
CHECK_N = 64
MAX_JOBS = 16
HINGE_SWEEP_DEG = 10.0
MAX_FINGER_POSES = 4096
COLLISION_ERROR = "a MuJoCo collision error"
STAND_IN_SEP = "~"
SWEPT_SUFFIX = "~swept"
# contype of an arm geom = conaffinity of a rover part: they meet each other and
# nothing else (arm-arm, rover-rover, the ground and loose objects never do)
ARM_BIT = 2
# the two model layouts (--layout): the defaults each one gives the flags left unset
LAYOUTS = {
    "sim": {"rover_body": "rover", "arm_root": "shoulder_link", "rover_parts": ",".join(ROVER_PARTS),
            "visual": "count", "root_voxel": ROOT_VOXEL_M, "lidar": True, "margin": MARGIN_M, "payload": "none"},
    # the CAD's collision hulls are the parts themselves, and the folded arm
    # rests 3 to 4 mm from the bin's front wall: a 10 mm margin would forbid
    # the robot's own driving pose
    # the robot's rule: a pose the table allows must stay clear with a can in the gripper
    "cad": {"rover_body": "rover_exterior,base", "arm_root": "shoulder", "rover_parts": "*",
            "visual": "skip", "root_voxel": 0.0, "lidar": False, "margin": CAD_MARGIN_M, "payload": "can"},
}
CAD_BODY = "rover_exterior"          # the body that tells the CAD scene apart
# a piece of a CAD part: <part>_NNN (a decomposition's hull) or <part>_envelope,
# after a part name that ends in digits
PIECE = re.compile(r"^(?P<part>.+_\d+)_(?:\d{3}|envelope)$")
EXPORTER_NAME = re.compile(r"^Open CASCADE STEP translator")    # a name the CAD exporter made up
MAX_PARTS_U8 = 255                   # cell_part in one byte while the table names this many parts or fewer
ARENA_BYTES = 256 << 20              # MuJoCo's arena per MjData: a pose deep in the rover makes 10000+ contacts
TOP_PAIRS = 100                      # pair_counts by name: the most frequent pairs (pair_cells holds them all)
TRAVEL_PAD_DEG = 3.0                 # --calibration: the grid reaches this far past each end of the calibrated travel
# --payload: what the gripper holds; a pose the table allows stays clear with it
PAYLOADS = {"can": (0.066, 0.120)}   # a soda can: diameter, height (m)
PAYLOAD_ORIENTATIONS = ("upright", "across")
PAD_BAND_M = 0.001                   # a finger's pad: its points within this of the finger's inner face
PUSH_STEP_M = 0.001                  # the payload leaves the hand in steps of this, then halved down to PUSH_TOL_M
PUSH_TOL_M = 0.00005
# the CAD names the gripper's parts after its working code, the first word
# (two capitals and two digits, sometimes after "MOSS "): the table says "gripper"
WORK_CODE = re.compile(r"^(?:MOSS )?[A-Z]{2}\d{2} +")
JOINT_MAP_BASIS = (
    "first alignment: model_zero_ticks 2048 and sign +1 on all five joints (the middle of each calibrated travel "
    "on the model's zero). Rendered on the SO-101 of the CAD scene with this map, the two poses recorded on the arm "
    "look right: drive (ticks pan 2857, lift 860, elbow 3206, flex 868, roll 3221 = pan +71.1, lift -104.4, "
    "elbow +101.8, flex -103.7, roll +103.1 deg) is the arm folded, the forearm back over the upper arm, the "
    "gripper straight up; look (ticks pan 2869, lift 1057, elbow 3208, flex 2018, roll 3220) is the forearm level, "
    "pointing ahead, the gripper forward and down (flex -2.6 deg). drive sits past the model's limits on the lift "
    "(4 deg), the elbow (5) and the wrist flex (9), look on the elbow (5): the real end stops lie past the model's "
    "(the reason for pad_deg). The pan zero is the least certain, about 15 deg either way. The final check is the "
    "operator's eyes on the robot: the same two poses, model against arm (docs/arm.md).")


def profile_lidar_xyz() -> tuple[float, float, float]:
    """The lidar in the rover body frame, from the profile: base_link is the
    lidar's floor projection and sits on the rotation centre (x = y = 0)."""
    from moss_dimos import robot_profile
    return (0.0, 0.0, float(robot_profile.current().lidar_height_m))


def sha256_of(path: Path) -> str:
    h = hashlib.sha256()
    with open(path, "rb") as fh:
        for chunk in iter(lambda: fh.read(1 << 20), b""):
            h.update(chunk)
    return h.hexdigest()


GEOMETRY_ARRAYS = ("names", "geom_type", "geom_size", "geom_pos", "geom_quat", "geom_bodyid", "geom_dataid",
                   "body_parentid", "body_pos", "body_quat", "jnt_type", "jnt_bodyid", "jnt_pos", "jnt_axis",
                   "jnt_range", "jnt_limited", "mesh_vertadr", "mesh_vertnum", "mesh_vert", "mesh_faceadr",
                   "mesh_facenum", "mesh_face")


def geometry_sha256(model) -> str:
    """The compiled model's geometry: names, bodies, joints, geoms and every
    mesh's vertices and faces. It changes with an included file or a new mesh
    export, which the top file's own sha256 does not see."""
    h = hashlib.sha256()
    for name in GEOMETRY_ARRAYS:
        arr = np.ascontiguousarray(getattr(model, name))
        h.update(name.encode() + str(arr.dtype).encode() + str(arr.shape).encode())
        h.update(arr.tobytes())
    return h.hexdigest()


def model_label(path: Path) -> str:
    """The model as written in the JSON: its file name, never its path (a
    path can name a private checkout; the two sha256 say which file)."""
    return Path(path).name


def layout_of(model_path: Path) -> str:
    """"cad" for the CAD scene (it has a body named rover_exterior), else "sim"."""
    import mujoco

    spec = mujoco.MjSpec.from_file(str(model_path))       # held while its bodies are read: they live in it
    bodies = {b.name for b in spec.bodies}
    return "cad" if CAD_BODY in bodies else "sim"


def part_key(name: str) -> str:
    """The part a geom belongs to: a stand-in or a swept volume answers to its
    source geom, a CAD piece to its part (col_part_0996_017 -> col_part_0996)."""
    name = name.removesuffix(SWEPT_SUFFIX)
    src, sep, k = name.rpartition(STAND_IN_SEP)
    if sep and k.isdigit():
        name = src
    m = PIECE.match(name)
    return m.group("part") if m else name


def public_name(name: str) -> str:
    """A CAD name as the table shows it: the gripper's working code (WORK_CODE)
    reads "gripper"."""
    m = WORK_CODE.match(name)
    if not m:
        return name
    rest = name[m.end():]
    return rest if rest.lower().startswith("gripper") else "gripper " + rest


def _rgba_key(value) -> tuple:
    try:
        return tuple(round(float(v), 3) for v in value)
    except (TypeError, ValueError):
        return ()


def load_names(path) -> dict[str, str]:
    """{part id: the name to show} from a parts manifest: {"parts": [{"id",
    "name", "path", "rig", "rgba"}], "visuals": [{"id", "rig", "rgba"}]}. A
    name the CAD exporter made up takes the name of the component that holds
    it (the path's next-to-last step); the gripper's working code reads
    "gripper" (public_name); a name several parts share gets the part's id in
    brackets. A visual mesh is named after the parts of its rigid group in
    its colour, "(visual mesh)" after it."""
    doc = json.loads(Path(path).read_text())
    parts = doc.get("parts") if isinstance(doc, dict) else None
    if not isinstance(parts, list):
        raise SystemExit(f"{path}: no \"parts\" list (--names takes a parts manifest)")
    raw, groups = {}, {}
    for p in parts:
        if not isinstance(p, dict) or not p.get("id"):
            continue
        name = str(p.get("name") or p["id"]).strip()
        steps = [s.rsplit(":", 1)[0] for s in str(p.get("path", "")).split("+") if s]
        if EXPORTER_NAME.match(name) and len(steps) >= 2:
            name = steps[-2]
        raw[str(p["id"])] = public_name(name)
        groups.setdefault((p.get("rig"), _rgba_key(p.get("rgba"))), []).append(str(p["id"]))
    shared = Counter(raw.values())
    out = {i: (f"{n} ({i})" if shared[n] > 1 else n) for i, n in raw.items()}
    for v in doc.get("visuals") or []:
        if not isinstance(v, dict) or not v.get("id"):
            continue
        ids = sorted(groups.get((v.get("rig"), _rgba_key(v.get("rgba"))), []))
        if ids:
            more = f" and {len(ids) - 1} more" if len(ids) > 1 else ""
            out[str(v["id"])] = f"{raw[ids[0]]}{more} (visual mesh)"
    return out


def display_name(names: dict, key: str) -> str:
    """A part key as shown: its manifest name (the key, or the key without its
    "col_" prefix, as the manifest's id), else the key."""
    if key in names:
        return names[key]
    if key.startswith("col_") and key[4:] in names:
        return names[key[4:]]
    return key


def joint_couplings(model) -> dict[int, tuple[int, list[float]]]:
    """{joint1: (joint2, polycoef)} for the model's joint equalities that tie a
    joint to another: joint1 - its qpos0 = poly(joint2 - its qpos0)."""
    import mujoco

    out = {}
    for e in range(model.neq):
        if int(model.eq_type[e]) != int(mujoco.mjtEq.mjEQ_JOINT) or not bool(model.eq_active0[e]):
            continue
        j1, j2 = int(model.eq_obj1id[e]), int(model.eq_obj2id[e])
        if j2 >= 0:
            out[j1] = (j2, [float(v) for v in model.eq_data[e][:5]])
    return out


def apply_couplings(model, qpos: np.ndarray, couplings: dict, joints) -> None:
    """Sets every joint of `joints` that a coupling ties to another from that
    other's value (MuJoCo applies equalities only when it steps)."""
    joints = set(joints)
    for _ in range(len(couplings) + 1):
        for j1, (j2, c) in couplings.items():
            if j1 not in joints:
                continue
            a1, a2 = int(model.jnt_qposadr[j1]), int(model.jnt_qposadr[j2])
            x = float(qpos[a2] - model.qpos0[a2])
            qpos[a1] = model.qpos0[a1] + sum(ck * x ** k for k, ck in enumerate(c))


def open_fingers(model, qpos: np.ndarray, loose: list[int], couplings: dict) -> dict[int, float]:
    """{finger joint: its value with the gripper open}: each finger joint at
    the end of its range that spreads the gripper the most (the bounding box
    of every geom the finger joints move, the other fingers as in qpos), and a
    joint a coupling ties to another finger joint following it. Ties go to the
    lower end."""
    import mujoco

    if not loose:
        return {}
    data = mujoco.MjData(model)
    q = np.array(qpos, float)
    moved = set()
    for j in loose:
        moved |= _subtree(model, int(model.jnt_bodyid[j]))
    local = {}
    for g in range(model.ngeom):
        if int(model.geom_bodyid[g]) in moved:
            try:
                local[g] = hull_points_local(model, g)
            except SystemExit:
                continue

    def spread(qq):
        data.qpos[:] = qq
        mujoco.mj_kinematics(model, data)
        if not local:
            return 0.0
        pts = np.concatenate([_to_world(data, g, p) for g, p in local.items()])
        return float(np.linalg.norm(pts.max(0) - pts.min(0)))

    followers = {j for j in loose if j in couplings and couplings[j][0] in loose}
    for j in loose:
        if j in followers:
            continue
        lo, hi = (float(v) for v in model.jnt_range[j]) if model.jnt_limited[j] else (-math.pi, math.pi)
        best = None
        for v in (lo, hi):
            qq = q.copy()
            qq[model.jnt_qposadr[j]] = v
            apply_couplings(model, qq, couplings, followers)
            s = spread(qq)
            if best is None or s > best[0] + 1e-9:
                best = (s, v)
        q[model.jnt_qposadr[j]] = best[1]
    apply_couplings(model, q, couplings, followers)
    return {j: float(q[model.jnt_qposadr[j]]) for j in loose}


def parse_payload(text) -> dict | None:
    """--payload: "none", "can" (66 mm across, 120 mm tall) or
    "cyl:<diameter mm>x<height mm>", as {"kind", "diameter_m", "height_m"}
    (None for none)."""
    t = str(text or "").strip().lower()
    if t in ("", "none"):
        return None
    if t in PAYLOADS:
        d, h = PAYLOADS[t]
        return {"kind": t, "diameter_m": d, "height_m": h}
    m = re.fullmatch(r"cyl:(\d+(?:\.\d+)?)x(\d+(?:\.\d+)?)", t)
    if not m or float(m.group(1)) <= 0 or float(m.group(2)) <= 0:
        raise SystemExit(f"--payload takes none, can or cyl:<diameter mm>x<height mm>, not {text!r}")
    return {"kind": "cyl", "diameter_m": float(m.group(1)) / 1000.0, "height_m": float(m.group(2)) / 1000.0}


def payload_label(payload: dict, orientation: str) -> str:
    """The payload as an arm part of the table: "payload can, upright"."""
    what = payload["kind"] if payload["kind"] != "cyl" else (
        f"cylinder {1000 * payload['diameter_m']:g} x {1000 * payload['height_m']:g} mm")
    return f"payload {what}, {orientation}"


def _frame_quat(z_axis, x_axis) -> list[float]:
    """The quaternion of the frame whose z is z_axis and whose x is x_axis
    (unit and perpendicular), in the frame they are written in."""
    import mujoco

    z, x = np.asarray(z_axis, float), np.asarray(x_axis, float)
    q = np.zeros(4)
    mujoco.mju_mat2Quat(q, np.column_stack([x, np.cross(z, x), z]).ravel())
    return [float(v) for v in q]


def grip_payload(model, qpos, loose, couplings, counts, payload: dict) -> dict:
    """Where a cylinder payload sits in the gripper, in the frame of the
    fingers' body (the hand). The gripper: two fingers on slide joints under
    one body (a finger a coupling ties to the other follows it). s, the
    slide axis, is the first finger's joint axis; a, the approach (where the
    fingers point), is the axis of the hand's frame perpendicular to s that
    points from the hand's own geoms to the fingers; c = s x a. A finger's
    pad is its geoms that reach the finger's inner face (its nearest point
    to the other finger along s, among its points past the hand's own geoms
    along a), within 1 mm, as far as they reach past the hand. The fingers close
    until the two faces are the payload's diameter apart (the slides at one
    value, a coupled joint through its coupling). The payload's centre lies
    midway between the faces, at the pads' middle across (c) and along the
    approach (a); from there it is pushed out along a (1 mm steps, then
    halved down to 0.05 mm) until it clears every other geom of the hand
    and of its fingers: a can cannot go into the palm. "upright" holds its
    axis along a (a can standing on the floor, picked from above), "across"
    along c (a can lying down, picked from the side). qpos: the open pose.
    counts(geom): an arm geom of the sweep. Returns the hand, the finger
    values that hold it, the gap, the axes, the pads and a pose per
    orientation (pos, axis, quat, how far it was pushed out and by what)."""
    import mujoco

    loose = list(loose)
    followers = {j for j in loose if j in couplings and couplings[j][0] in loose}
    slides = [j for j in loose if int(model.jnt_type[j]) == int(mujoco.mjtJoint.mjJNT_SLIDE)]
    fingers = sorted({int(model.jnt_bodyid[j]) for j in slides})
    parents = {int(model.body_parentid[b]) for b in fingers}
    drivers = [j for j in slides if j not in followers]
    if len(fingers) != 2 or len(parents) != 1 or not drivers:
        raise SystemExit("--payload: the gripper needs two fingers on slide joints under one body, swept (not "
                         "pinned with --fix); the arm's unswept joints here: "
                         + (", ".join(joint_name(model, j) for j in loose) or "none"))
    hand = parents.pop()
    for j in drivers:
        if not model.jnt_limited[j]:
            raise SystemExit(f"--payload: finger {joint_name(model, j)!r} slides without limits")
    lo = max(float(model.jnt_range[j][0]) for j in drivers)
    hi = min(float(model.jnt_range[j][1]) for j in drivers)
    sub = _subtree(model, hand)
    geoms = [g for g in range(model.ngeom) if int(model.geom_bodyid[g]) in sub and counts(g)]
    local = {g: hull_points_local(model, g) for g in geoms}
    side = [_subtree(model, f) for f in fingers]
    on = [[g for g in geoms if int(model.geom_bodyid[g]) in side[k]] for k in (0, 1)]
    own = [g for g in geoms if int(model.geom_bodyid[g]) == hand]
    if not on[0] or not on[1]:
        raise SystemExit("--payload: a finger carries no geom")
    data = mujoco.MjData(model)

    def pose(v):
        q = np.array(qpos, float)
        for j in drivers:
            q[model.jnt_qposadr[j]] = v
        apply_couplings(model, q, couplings, followers)
        return q

    def points(q):
        data.qpos[:] = q
        mujoco.mj_kinematics(model, data)
        return {g: _to_body(data, hand, _to_world(data, g, local[g])) for g in geoms}

    pts = points(np.array(qpos, float))
    j0 = next(j for j in slides if int(model.jnt_bodyid[j]) in side[0])
    s = np.asarray(data.xmat[hand]).reshape(3, 3).T @ np.asarray(data.xaxis[j0], float)
    s /= np.linalg.norm(s)

    def cat(gs, pp):
        return np.concatenate([pp[g] for g in gs])

    if float((cat(on[0], pts) @ s).mean()) > float((cat(on[1], pts) @ s).mean()):
        s = -s                      # s points from the first finger to the second
    fp = cat(on[0] + on[1], pts)
    ref = (fp.min(0) + fp.max(0)) / 2.0
    if own:
        hp = cat(own, pts)
        ref = ref - (hp.min(0) + hp.max(0)) / 2.0
    ref = ref - (ref @ s) * s
    cands = [e for e in np.vstack([np.eye(3), -np.eye(3)]) if abs(float(e @ s)) < 1e-3]
    a = max(cands, key=lambda e: float(e @ ref)) if cands else ref / max(float(np.linalg.norm(ref)), 1e-12)
    a = np.asarray(a, float)
    if float(a @ ref) <= 0:
        raise SystemExit("--payload: the fingers do not reach past the hand's own geoms: no approach direction")
    s = s - (s @ a) * a
    s /= np.linalg.norm(s)
    c = np.cross(s, a)
    palm = max(float((pts[g] @ a).max()) for g in own) if own else -math.inf

    def faces(v):
        pp = points(pose(v))
        out = []
        for k in (0, 1):
            proj = []
            for g in on[k]:
                x = pp[g][pp[g] @ a > palm + 1e-9]
                if len(x):
                    proj.append((g, x, x @ s))
            if not proj:
                raise SystemExit("--payload: a finger has nothing past the hand's own geoms")
            f = max(float(d.max()) for _, _, d in proj) if k == 0 else min(float(d.min()) for _, _, d in proj)
            # the pad: every geom with a point on the face, as far as it reaches past the hand
            out.append((f, [(g, x) for g, x, d in proj
                            if np.any((d >= f - PAD_BAND_M) if k == 0 else (d <= f + PAD_BAND_M))]))
        return out

    def gap(v):
        (fa, _), (fb, _) = faces(v)
        return fb - fa

    dia, height = float(payload["diameter_m"]), float(payload["height_m"])
    g_lo, g_hi = gap(lo), gap(hi)
    if not min(g_lo, g_hi) - 1e-9 <= dia <= max(g_lo, g_hi) + 1e-9:
        raise SystemExit(f"--payload: {1000 * dia:g} mm across does not fit: the pads open from "
                         f"{1000 * min(g_lo, g_hi):.1f} to {1000 * max(g_lo, g_hi):.1f} mm")
    v = lo if g_hi == g_lo else lo + (dia - g_lo) * (hi - lo) / (g_hi - g_lo)
    if abs(gap(v) - dia) > 1e-7:          # the gap is not linear in the slide: halve the interval
        a_, b_, ga = lo, hi, g_lo
        for _ in range(60):
            m_ = (a_ + b_) / 2.0
            gm = gap(m_)
            if (gm - dia) * (ga - dia) <= 0:
                b_ = m_
            else:
                a_, ga = m_, gm
        v = (a_ + b_) / 2.0
    (fa, pad_a), (fb, pad_b) = faces(v)
    q_hold = pose(v)
    pad_ids = sorted({g for g, _ in pad_a + pad_b})

    def middle(pad, axis):
        x = np.concatenate([x for _, x in pad]) @ axis
        return (float(x.min()) + float(x.max())) / 2.0

    along_a = (middle(pad_a, a) + middle(pad_b, a)) / 2.0
    across_c = (middle(pad_a, c) + middle(pad_b, c)) / 2.0
    pad_pts = np.concatenate([x for _, x in pad_a + pad_b])
    pad_range = [float((pad_pts @ a).min()), float((pad_pts @ a).max())]
    centre0 = s * (fa + fb) / 2.0 + a * along_a + c * across_c

    # the hand and its fingers at the holding pose, the pads left out (the
    # payload touches them), in a small model of their own: MuJoCo says
    # where the payload goes into one of them
    data.qpos[:] = q_hold
    mujoco.mj_kinematics(model, data)
    rot = np.asarray(data.xmat[hand]).reshape(3, 3)
    probe = mujoco.MjSpec()
    if hasattr(mujoco.mjtDisableBit, "mjDSBL_NATIVECCD"):
        probe.option.disableflags |= int(mujoco.mjtDisableBit.mjDSBL_NATIVECCD)
    body = probe.worldbody.add_body(name="hand")
    others = [g for g in geoms if g not in pad_ids]
    mesh_t = int(mujoco.mjtGeom.mjGEOM_MESH)
    for k, g in enumerate(others):
        pos = rot.T @ (np.asarray(data.geom_xpos[g]) - data.xpos[hand])
        quat = np.zeros(4)
        mujoco.mju_mat2Quat(quat, (rot.T @ np.asarray(data.geom_xmat[g]).reshape(3, 3)).ravel())
        if int(model.geom_type[g]) == mesh_t:
            mesh = probe.add_mesh(name=f"m{k}", uservert=local[g].ravel().tolist())
            mesh.maxhullvert = -1
            body.add_geom(name=f"g{k}", type=mujoco.mjtGeom.mjGEOM_MESH, meshname=f"m{k}",
                          pos=pos.tolist(), quat=quat.tolist())
        else:
            body.add_geom(name=f"g{k}", type=mujoco.mjtGeom(int(model.geom_type[g])),
                          size=[float(x) for x in model.geom_size[g]], pos=pos.tolist(), quat=quat.tolist())
    body.add_geom(name="payload", type=mujoco.mjtGeom.mjGEOM_CYLINDER, size=[dia / 2.0, height / 2.0, 0.0])
    pm = probe.compile()
    pdata = mujoco.MjData(pm)
    cid = mujoco.mj_name2id(pm, mujoco.mjtObj.mjOBJ_GEOM, "payload")
    pm.geom_sameframe[cid] = 0         # its pos and quat change below: kinematics must read them
    probe_ids = [mujoco.mj_name2id(pm, mujoco.mjtObj.mjOBJ_GEOM, f"g{k}") for k in range(len(others))]

    def blocker(t):
        pm.geom_pos[cid] = centre0 + t * a
        mujoco.mj_kinematics(pm, pdata)
        for k, gk in enumerate(probe_ids):
            if mujoco.mj_geomDistance(pm, pdata, cid, gk, 0.0, None) < 0:
                return others[k]
        return None

    poses = {}
    for o in PAYLOAD_ORIENTATIONS:
        axis = a if o == "upright" else c
        quat = _frame_quat(axis, s)
        pm.geom_quat[cid] = quat
        t, by = 0.0, blocker(0.0)
        last = by
        while by is not None:
            t += PUSH_STEP_M
            if t > height + dia:
                raise SystemExit(f"--payload: held {o}, it goes into {geom_name(model, last)} wherever it sits "
                                 "along the approach")
            by = blocker(t)
            last = by if by is not None else last
        if t > 0:
            t_in, t_out = t - PUSH_STEP_M, t
            while t_out - t_in > PUSH_TOL_M:
                mid = (t_in + t_out) / 2.0
                hit = blocker(mid)
                if hit is None:
                    t_out = mid
                else:
                    t_in, last = mid, hit
            t = t_out
        centre = centre0 + t * a
        ca = float(centre @ a)
        held = (ca - height / 2.0 < pad_range[1] and ca + height / 2.0 > pad_range[0]) if o == "upright" \
            else pad_range[0] <= ca <= pad_range[1]
        if not held:
            raise SystemExit(f"--payload: held {o}, {geom_name(model, last)} pushes it off the pads")
        poses[o] = {"pos": centre, "axis": axis, "quat": quat, "pushed_out_m": t,
                    "pushed_by": last if t > 0 else None}
    return {"hand": hand, "holding": {j: float(q_hold[model.jnt_qposadr[j]]) for j in loose}, "slide": v,
            "gap_m": fb - fa, "gap_range_m": [min(g_lo, g_hi), max(g_lo, g_hi)], "s": s, "a": a, "c": c,
            "pads": pad_ids, "pad_range_m": pad_range, "poses": poses}


def _matches(name: str, patterns) -> bool:
    return bool(name) and any(fnmatch.fnmatchcase(name, p) for p in patterns)


def _subtree(model, root: int) -> set[int]:
    out = {root}
    for b in range(root + 1, model.nbody):
        if int(model.body_parentid[b]) in out:
            out.add(b)
    return out


def _name(model, obj, i: int) -> str:
    import mujoco
    return mujoco.mj_id2name(model, obj, i) or ""


def geom_name(model, gid: int) -> str:
    import mujoco
    name = _name(model, mujoco.mjtObj.mjOBJ_GEOM, gid)
    if name:
        return name
    body = _name(model, mujoco.mjtObj.mjOBJ_BODY, int(model.geom_bodyid[gid])) or f"body{model.geom_bodyid[gid]}"
    return f"{body}/geom{gid}"


def joint_name(model, jid: int) -> str:
    import mujoco
    return _name(model, mujoco.mjtObj.mjOBJ_JOINT, jid) or f"joint{jid}"


# ── geometry in numpy ──

def _mesh_arrays(model, gid: int):
    mid = int(model.geom_dataid[gid])
    va, vn = int(model.mesh_vertadr[mid]), int(model.mesh_vertnum[mid])
    fa, fn = int(model.mesh_faceadr[mid]), int(model.mesh_facenum[mid])
    return np.asarray(model.mesh_vert[va:va + vn], float), np.asarray(model.mesh_face[fa:fa + fn], int)


def hull_points_local(model, gid: int) -> np.ndarray:
    """Points whose convex hull holds the geom, in its own frame: a mesh's
    vertices and a box's corners (exact), the bounding box of the other
    primitives (larger)."""
    import mujoco

    t = int(model.geom_type[gid])
    s = np.asarray(model.geom_size[gid], float)
    if t == int(mujoco.mjtGeom.mjGEOM_MESH):
        return _mesh_arrays(model, gid)[0]
    if t == int(mujoco.mjtGeom.mjGEOM_BOX):
        half = s
    elif t == int(mujoco.mjtGeom.mjGEOM_SPHERE):
        half = np.array([s[0]] * 3)
    elif t == int(mujoco.mjtGeom.mjGEOM_CAPSULE):
        half = np.array([s[0], s[0], s[0] + s[1]])
    elif t == int(mujoco.mjtGeom.mjGEOM_CYLINDER):
        half = np.array([s[0], s[0], s[1]])
    elif t == int(mujoco.mjtGeom.mjGEOM_ELLIPSOID):
        half = s
    else:
        raise SystemExit(f"geom {geom_name(model, gid)}: type {t} cannot be swept (box, sphere, capsule, "
                         "cylinder, ellipsoid or mesh)")
    return np.array(list(itertools.product(*[(-h, h) for h in half])), float)


def _triangle_points(v: np.ndarray, f: np.ndarray, spacing: float) -> np.ndarray:
    """Points on every triangle, along its two edges from its first corner in
    steps no longer than `spacing`: every point of the triangle lies within
    `spacing` of one of them."""
    a, b, c = v[f[:, 0]], v[f[:, 1]], v[f[:, 2]]
    k1 = np.maximum(1, np.ceil(np.linalg.norm(b - a, axis=1) / spacing)).astype(int)
    k2 = np.maximum(1, np.ceil(np.linalg.norm(c - a, axis=1) / spacing)).astype(int)
    out = [v]
    for n1, n2 in set(zip(k1.tolist(), k2.tolist())):
        if n1 == 1 and n2 == 1:
            continue
        sel = (k1 == n1) & (k2 == n2)
        u, w = np.meshgrid(np.arange(n1 + 1) / n1, np.arange(n2 + 1) / n2, indexing="ij")
        keep = (u + w) <= 1.0 + 1e-12
        u, w = u[keep], w[keep]
        p = a[sel][:, None, :] + u[None, :, None] * (b[sel] - a[sel])[:, None, :] \
            + w[None, :, None] * (c[sel] - a[sel])[:, None, :]
        out.append(p.reshape(-1, 3))
    return np.concatenate(out)


def mesh_surface_points(model, gid: int, spacing: float) -> np.ndarray:
    """Points on a mesh geom's surface, every point of it within `spacing` of
    one of them, in the geom's frame."""
    return _triangle_points(*_mesh_arrays(model, gid), spacing)


def _to_world(data, gid: int, local: np.ndarray) -> np.ndarray:
    return local @ np.asarray(data.geom_xmat[gid]).reshape(3, 3).T + data.geom_xpos[gid]


def _to_body(data, bid: int, world: np.ndarray) -> np.ndarray:
    return (world - data.xpos[bid]) @ np.asarray(data.xmat[bid]).reshape(3, 3)


class _Solid:
    """Point-in-geom (inflated by tol) for the rover parts that count as they are."""

    def __init__(self, model, data, gid: int):
        import mujoco

        self.type = int(model.geom_type[gid])
        self.size = np.asarray(model.geom_size[gid], float)
        self.pos = np.array(data.geom_xpos[gid])
        self.rot = np.array(data.geom_xmat[gid]).reshape(3, 3)
        self.lo = self.hi = self.planes = None
        try:
            corners = _to_world(data, gid, hull_points_local(model, gid))
        except SystemExit:          # a plane or a height field covers nothing
            return
        self.lo, self.hi = corners.min(0), corners.max(0)
        if self.type == int(mujoco.mjtGeom.mjGEOM_MESH):
            try:
                from scipy.spatial import ConvexHull
                self.planes = ConvexHull(corners).equations
            except Exception:          # no scipy, or a flat mesh: this part covers nothing
                self.planes = None

    def inside(self, p: np.ndarray, tol: float) -> np.ndarray:
        import mujoco

        out = np.zeros(len(p), bool)
        if self.lo is None:
            return out
        near = np.all((p >= self.lo - tol) & (p <= self.hi + tol), axis=1)
        if not near.any():
            return out
        q = p[near]
        loc = (q - self.pos) @ self.rot
        s, t = self.size, self.type
        if t == int(mujoco.mjtGeom.mjGEOM_BOX):
            ok = np.all(np.abs(loc) <= s + tol, axis=1)
        elif t == int(mujoco.mjtGeom.mjGEOM_SPHERE):
            ok = np.linalg.norm(loc, axis=1) <= s[0] + tol
        elif t == int(mujoco.mjtGeom.mjGEOM_CAPSULE):
            axial = np.zeros_like(loc)
            axial[:, 2] = np.clip(loc[:, 2], -s[1], s[1])
            ok = np.linalg.norm(loc - axial, axis=1) <= s[0] + tol
        elif t == int(mujoco.mjtGeom.mjGEOM_CYLINDER):
            ok = (np.hypot(loc[:, 0], loc[:, 1]) <= s[0] + tol) & (np.abs(loc[:, 2]) <= s[1] + tol)
        elif t == int(mujoco.mjtGeom.mjGEOM_ELLIPSOID):
            ok = np.sum((loc / (s + tol)) ** 2, axis=1) <= 1.0
        elif t == int(mujoco.mjtGeom.mjGEOM_MESH) and self.planes is not None:
            ok = np.all(q @ self.planes[:, :3].T + self.planes[:, 3] <= tol, axis=1)
        else:
            ok = np.zeros(len(q), bool)
        out[near] = ok
        return out


def merge_voxels(grid: np.ndarray) -> list[tuple[np.ndarray, np.ndarray]]:
    """Boxes (first voxel index, last voxel index) covering every True voxel
    and nothing else: runs along x, grown along y, then along z."""
    g = grid.copy()
    boxes = []
    for x, y, z in np.argwhere(g):
        if not g[x, y, z]:
            continue
        x2 = x
        while x2 + 1 < g.shape[0] and g[x2 + 1, y, z]:
            x2 += 1
        y2 = y
        while y2 + 1 < g.shape[1] and g[x:x2 + 1, y2 + 1, z].all():
            y2 += 1
        z2 = z
        while z2 + 1 < g.shape[2] and g[x:x2 + 1, y:y2 + 1, z2 + 1].all():
            z2 += 1
        g[x:x2 + 1, y:y2 + 1, z:z2 + 1] = False
        boxes.append((np.array([x, y, z]), np.array([x2, y2, z2])))
    return boxes


def voxel_boxes(points: np.ndarray, voxel: float) -> list[tuple[np.ndarray, np.ndarray]]:
    """Boxes [(centre, half size)] made of whole voxels that hold every point
    within half a voxel (per axis) of `points`: sampled every half voxel, a
    surface is covered whole."""
    if not len(points):
        return []
    h = voxel / 2.0
    corners = np.array(list(itertools.product((-h, h), repeat=3)))
    idx = np.unique(np.floor((points[:, None, :] + corners[None, :, :]).reshape(-1, 3) / voxel).astype(np.int64),
                    axis=0)
    base = idx.min(0)
    grid = np.zeros(idx.max(0) - base + 1, bool)
    grid[tuple((idx - base).T)] = True
    out = []
    for a, b in merge_voxels(grid):
        lo, hi = (a + base) * voxel, (b + base + 1) * voxel
        out.append(((lo + hi) / 2.0, (hi - lo) / 2.0))
    return out


def rover_mesh_stand_ins(model, data, gid: int, solids: list[_Solid], rover_bid: int, voxel: float, tol: float):
    """The boxes standing in for one rover mesh, in the rover body's frame,
    and its report: the share of its sampled surface the solids miss (by
    more than tol), the extent of that share, the number of boxes."""
    local = mesh_surface_points(model, gid, voxel / 2.0)
    pts = _to_world(data, gid, local)
    lo, hi = pts.min(0) - tol, pts.max(0) + tol
    covered = np.zeros(len(pts), bool)
    for s in solids:
        if s.lo is not None and np.all(s.hi >= lo) and np.all(s.lo <= hi):
            covered |= s.inside(pts, tol)
    report = {"outside_fraction": round(float(1.0 - covered.mean()), 4), "boxes": 0}
    out = _to_body(data, rover_bid, pts[~covered])
    if not len(out):
        return [], report
    report["outside_extent_m"] = [[round(float(v), 4) for v in out.min(0)], [round(float(v), 4) for v in out.max(0)]]
    boxes = voxel_boxes(out, voxel)
    report["boxes"] = len(boxes)
    return boxes, report


# ── the model, ready to sweep ──

class Prepared:
    """The compiled models with their contact masks, and what the sweep needs:
    model (every link but the root), root_model (the root link's geoms or
    stand-ins only), the arm and rover geom ids (the same in both), the part
    key of each rover geom (part_label, in part_ids order) and of each arm
    geom (arm_label, by name), and the name each part key shows (display)."""

    def __init__(self, **kw):
        self.__dict__.update(kw)


def _unique_name(taken: set, want: str) -> str:
    name, k = want, 1
    while name in taken:
        name, k = f"{want}_{k}", k + 1
    taken.add(name)
    return name


def body_list(value) -> list[str]:
    """--rover-body as a list: "a,b" or a sequence of names."""
    if isinstance(value, str):
        return [b.strip() for b in value.split(",") if b.strip()]
    return [str(b) for b in value]


def prepare(model_path: Path, *, arm_root: str = "shoulder_link", rover_body="rover",
            rover_parts=ROVER_PARTS, exclude=(), margin: float = MARGIN_M, lidar_xyz=None,
            joints=JOINTS, fixed=None, fingers: str = "swept", voxel: float = VOXEL_M,
            root_voxel: float = ROOT_VOXEL_M, root_margin: float = ROOT_MARGIN_M, cover_tol: float = COVER_TOL_M,
            visual: str = "count", names=None, keep_inside: bool = False, stand_ins=None,
            payload=None) -> Prepared:
    """Load the MJCF, add the lidar when asked and the model has none, the
    stand-ins of the rover's meshes and of the root link's (root_voxel > 0),
    the fingers' swept volumes, and set the contact masks so that
    mj_collision reports arm <-> rover-part contacts only, within `margin`.
    rover_body: one body or several ("a,b"). visual="skip" leaves out the
    geoms the model makes visual only (contype and conaffinity 0), except on
    a body that has no other geom to stand for them (no collision geom):
    there they count. names:
    {part id: name} (load_names). A rover geom held whole inside another one
    that counts as it is adds no forbidden pose (an arm geom near it is
    nearer the other): it is left out unless keep_inside. fingers="real"
    keeps the finger geoms as they are (the finger check). stand_ins: from
    an earlier prepare of the same model (skips the sampling). payload
    (parse_payload): a cylinder held in the gripper (grip_payload), once
    per orientation, on the fingers' body; payload_model sweeps those
    cylinders alone, the fingers at the gap that holds it."""
    import mujoco

    if visual not in ("count", "skip"):
        raise SystemExit(f"--visual takes count or skip, not {visual!r}")
    fixed = dict(fixed or {})
    names = dict(names or {})
    rover_names = body_list(rover_body)
    if not rover_names:
        raise SystemExit("--rover-body names no body")
    spec = mujoco.MjSpec.from_file(str(model_path))
    geometry = geometry_sha256(spec.compile())
    if spec.body(arm_root) is None:
        raise SystemExit(f"no body named {arm_root!r} in {model_path} (--arm-root)")
    for rb in rover_names:
        if spec.body(rb) is None:
            raise SystemExit(f"no body named {rb!r} in {model_path} (--rover-body)")
    rover = spec.body(rover_names[0])
    # a mesh collides as its whole convex hull: a cap on its vertices would
    # cut corners off the part
    hull_cap_lifted = 0
    for mesh in spec.meshes:
        if mesh.maxhullvert != -1:
            mesh.maxhullvert = -1
            hull_cap_lifted += 1
    taken = {g.name for g in spec.geoms if g.name} | {m.name for m in spec.meshes if m.name}
    for k, g in enumerate(spec.geoms):
        if not g.name:
            g.name = _unique_name(taken, f"{g.parent.name or 'body'}_geom{k}")
    lidar = {"added": False}
    if lidar_xyz is not None and not any(g.name == LIDAR_NAME for g in spec.geoms):
        rover.add_geom(name=LIDAR_NAME, type=mujoco.mjtGeom.mjGEOM_CYLINDER,
                       size=[LIDAR_RADIUS_M, LIDAR_HEIGHT_M / 2.0, 0.0], pos=[float(v) for v in lidar_xyz])
        taken.add(LIDAR_NAME)
        lidar = {"added": True, "pos_m": [round(float(v), 6) for v in lidar_xyz],
                 "radius_m": LIDAR_RADIUS_M, "height_m": LIDAR_HEIGHT_M}
    # the masks below decide every contact; the parent filter would hide an
    # arm root hung straight on a rover body
    spec.option.disableflags |= int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT)
    spec.option.disableflags &= ~int(mujoco.mjtDisableBit.mjDSBL_CONTACT)
    # MuJoCo's own convex collider (3.9) reports no contact for a geom held
    # whole inside a mesh's hull (a stand-in box inside an arm link, a small
    # part inside the gripper's swept volume); the libccd one does
    if hasattr(mujoco.mjtDisableBit, "mjDSBL_NATIVECCD"):
        spec.option.disableflags |= int(mujoco.mjtDisableBit.mjDSBL_NATIVECCD)
    spec.option.enableflags &= ~int(mujoco.mjtEnableBit.mjENBL_OVERRIDE)
    # a pose deep in the rover makes thousands of contacts: room for them
    # (the arena is reserved, not touched, until a pose needs it)
    if spec.memory < ARENA_BYTES:
        spec.memory = ARENA_BYTES

    m0 = spec.compile()
    d0 = mujoco.MjData(m0)
    for name, value in fixed.items():
        jid = mujoco.mj_name2id(m0, mujoco.mjtObj.mjOBJ_JOINT, name)
        if jid < 0:
            raise SystemExit(f"--fix: no joint named {name!r}")
        d0.qpos[m0.jnt_qposadr[jid]] = float(value)
    mujoco.mj_kinematics(m0, d0)
    BODY = mujoco.mjtObj.mjOBJ_BODY
    root_bid = mujoco.mj_name2id(m0, BODY, arm_root)
    rover_bids = [mujoco.mj_name2id(m0, BODY, rb) for rb in rover_names]
    arm_bodies = _subtree(m0, root_bid)
    # the mount: the bodies between the arm root and the nearest listed rover body above it
    chain, b = [], int(m0.body_parentid[root_bid])
    while b not in rover_bids and b != 0:
        chain.append(b)
        b = int(m0.body_parentid[b])
    mount_bodies = chain if b in rover_bids else []
    owner: dict[int, int] = {}      # rover body -> the listed body whose frame its stand-ins use
    for rb in rover_bids:
        for b in _subtree(m0, rb):
            owner.setdefault(b, rb)
    rover_bodies = set(owner) - arm_bodies - set(mount_bodies)
    # the model's own exclusions between an arm body and a rover body stay
    # (an interface the model declares, such as a base and the shoulder that
    # turns on it); the others and every explicit pair go
    kept_excludes = []
    for elem in list(spec.excludes):
        b1, b2 = mujoco.mj_name2id(m0, BODY, elem.bodyname1), mujoco.mj_name2id(m0, BODY, elem.bodyname2)
        if (b1 in arm_bodies and b2 in rover_bodies) or (b2 in arm_bodies and b1 in rover_bodies):
            kept_excludes.append([elem.bodyname1, elem.bodyname2])
        else:
            spec.delete(elem)
    for elem in list(spec.pairs):
        spec.delete(elem)
    gname = {g: geom_name(m0, g) for g in range(m0.ngeom)}
    mesh_t = int(mujoco.mjtGeom.mjGEOM_MESH)
    prim_t = {int(mujoco.mjtGeom.mjGEOM_BOX), int(mujoco.mjtGeom.mjGEOM_SPHERE), int(mujoco.mjtGeom.mjGEOM_CAPSULE),
              int(mujoco.mjtGeom.mjGEOM_CYLINDER), int(mujoco.mjtGeom.mjGEOM_ELLIPSOID)}

    def visual_only(g: int) -> bool:
        return int(m0.geom_contype[g]) == 0 and int(m0.geom_conaffinity[g]) == 0

    # a body with collision geoms: they stand for its visual meshes
    collides = {int(m0.geom_bodyid[g]) for g in range(m0.ngeom) if not visual_only(g)}

    def is_visual(g: int) -> bool:
        return visual == "skip" and visual_only(g) and int(m0.geom_bodyid[g]) in collides

    # the visual meshes that count with --visual skip: nothing else stands for them
    visual_kept = [gname[g] for g in range(m0.ngeom)
                   if visual == "skip" and visual_only(g) and not is_visual(g)
                   and int(m0.geom_bodyid[g]) in arm_bodies | rover_bodies]

    # the rover: as they are, or through stand-ins
    as_is, meshes, unused = [], [], []
    visual_out = {"arm": 0, "rover": 0}
    for g in range(m0.ngeom):
        bid = int(m0.geom_bodyid[g])
        if bid in arm_bodies and is_visual(g):
            visual_out["arm"] += 1
        if bid not in rover_bodies:
            continue
        if is_visual(g):
            visual_out["rover"] += 1
        elif _matches(gname[g], exclude):
            unused.append(gname[g])
        elif _matches(gname[g], rover_parts) or int(m0.geom_type[g]) in prim_t:
            as_is.append(g)
        elif int(m0.geom_type[g]) == mesh_t:
            meshes.append(g)
        else:
            unused.append(gname[g])
    inside = {} if keep_inside else held_inside(m0, d0, as_is)
    if inside:
        as_is = [g for g in as_is if g not in inside]
    root_geoms = [g for g in range(m0.ngeom) if int(m0.geom_bodyid[g]) == root_bid and not is_visual(g)]
    root_meshes = [g for g in root_geoms if int(m0.geom_type[g]) == mesh_t] if root_voxel > 0 else []
    if stand_ins is None:
        stand_ins = {"voxel_m": voxel, "root_voxel_m": root_voxel, "cover_tol_m": cover_tol,
                     "per_mesh": {}, "boxes": [], "root": {}, "root_boxes": []}
        solids = [_Solid(m0, d0, g) for g in as_is] if meshes and voxel > 0 else []
        for g in meshes:
            if voxel > 0:
                frame = owner[int(m0.geom_bodyid[g])]
                boxes, rep = rover_mesh_stand_ins(m0, d0, g, solids, frame, voxel, cover_tol)
                stand_ins["per_mesh"][gname[g]] = rep
                fname = _name(m0, BODY, frame)
                stand_ins["boxes"].extend((gname[g], fname, c, h) for c, h in boxes)
            else:
                stand_ins["per_mesh"][gname[g]] = {"left_out": "--stand-in-voxel 0"}
        for g in root_meshes:
            local = mesh_surface_points(m0, g, root_voxel / 2.0)
            boxes = voxel_boxes(_to_body(d0, root_bid, _to_world(d0, g, local)), root_voxel)
            stand_ins["root"][gname[g]] = len(boxes)
            stand_ins["root_boxes"].extend((gname[g], c, h) for c, h in boxes)
    if voxel <= 0:
        unused += [gname[g] for g in meshes]
    label = {gname[g]: part_key(gname[g]) for g in as_is}
    for k, (src, fname, c, h) in enumerate(stand_ins["boxes"]):
        name = _unique_name(taken, f"{src}{STAND_IN_SEP}{k}")
        spec.body(fname).add_geom(name=name, type=mujoco.mjtGeom.mjGEOM_BOX, size=[float(v) for v in h],
                                  pos=[float(v) for v in c], group=3, rgba=[0, 0, 0, 0])
        label[name] = part_key(src)
    part_names = list(label)
    if not part_names:
        raise SystemExit(f"no rover part on {', '.join(rover_names)!r}")
    root_spec = spec.body(arm_root)
    root_names = [gname[g] for g in root_geoms if g not in root_meshes]
    arm_label = {n: part_key(n) for n in root_names}
    for k, (src, c, h) in enumerate(stand_ins["root_boxes"]):
        name = _unique_name(taken, f"{src}{STAND_IN_SEP}{k}")
        root_spec.add_geom(name=name, type=mujoco.mjtGeom.mjGEOM_BOX, size=[float(v) for v in h],
                           pos=[float(v) for v in c], group=3, rgba=[0, 0, 0, 0])
        root_names.append(name)
        arm_label[name] = part_key(src)

    # the other links: as they are, the fingers' geoms as swept volumes
    swept_j = set()
    for j in joints:
        jid = mujoco.mj_name2id(m0, mujoco.mjtObj.mjOBJ_JOINT, j)
        if jid < 0:
            raise SystemExit(f"no joint named {j!r} in the model")
        swept_j.add(jid)
    movable = (int(mujoco.mjtJoint.mjJNT_SLIDE), int(mujoco.mjtJoint.mjJNT_HINGE))
    loose = []
    for j in range(m0.njnt):
        if int(m0.jnt_bodyid[j]) not in arm_bodies or j in swept_j or joint_name(m0, j) in fixed:
            continue
        if int(m0.jnt_type[j]) not in movable:
            raise SystemExit(f"arm joint {joint_name(m0, j)!r} is a ball or free joint: pin it with --fix")
        if int(m0.jnt_bodyid[j]) == root_bid:
            raise SystemExit(f"joint {joint_name(m0, j)!r} on the root link is not swept: sweep it or pin it with --fix")
        loose.append(j)
    couplings = joint_couplings(m0)
    open_pose = open_fingers(m0, d0.qpos, loose, couplings)
    grip = None
    if payload:
        q_open = np.array(d0.qpos)
        for j, v in open_pose.items():
            q_open[m0.jnt_qposadr[j]] = v
        grip = grip_payload(m0, q_open, loose, couplings,
                            lambda g: int(m0.geom_bodyid[g]) in arm_bodies and not is_visual(g), payload)
    body_joints: dict[int, list[int]] = {}
    for j in range(m0.njnt):
        body_joints.setdefault(int(m0.jnt_bodyid[j]), []).append(j)
    link_names, replaced = [], []
    for g in range(m0.ngeom):
        bid = int(m0.geom_bodyid[g])
        if bid not in arm_bodies or bid == root_bid or is_visual(g):
            continue
        chain, b = [], bid           # bodies from the geom's up to the arm root
        while True:
            chain.append(b)
            if b == root_bid:
                break
            b = int(m0.body_parentid[b])
        on_chain = [(b, j) for b in chain for j in body_joints.get(b, []) if j in loose]
        if not on_chain or fingers == "real":
            link_names.append(gname[g])
            arm_label[gname[g]] = part_key(gname[g])
            continue
        top = max(chain.index(b) for b, _ in on_chain)      # the body of the loose joint nearest the root
        if any(j in swept_j for b in chain[:top + 1] for j in body_joints.get(b, [])):
            raise SystemExit(f"geom {gname[g]}: a swept joint lies below an unswept one; sweep it or pin it with --fix")
        frame = int(m0.body_parentid[chain[top]])
        values = []
        for _, j in on_chain:
            lo, hi = (float(v) for v in m0.jnt_range[j]) if m0.jnt_limited[j] else (None, None)
            if int(m0.jnt_type[j]) == int(mujoco.mjtJoint.mjJNT_SLIDE):
                if lo is None:
                    raise SystemExit(f"finger {joint_name(m0, j)!r} slides without limits: pin it with --fix")
                values.append((lo, hi))
            else:
                if lo is None:
                    lo, hi = -math.pi, math.pi
                n = int(max(2, math.ceil(math.degrees(hi - lo) / HINGE_SWEEP_DEG) + 1))
                values.append(tuple(np.linspace(lo, hi, n)))
        if math.prod(len(v) for v in values) > MAX_FINGER_POSES:
            raise SystemExit(f"geom {gname[g]}: too many finger poses to sweep; pin some with --fix")
        local = hull_points_local(m0, g)
        d = mujoco.MjData(m0)
        pts = []
        for combo in itertools.product(*values):
            d.qpos[:] = d0.qpos
            for (_, j), v in zip(on_chain, combo):
                d.qpos[m0.jnt_qposadr[j]] = v
            mujoco.mj_kinematics(m0, d)
            pts.append(_to_body(d, frame, _to_world(d, g, local)))
        mesh = _unique_name(taken, re.sub(r"[^A-Za-z0-9_.-]", "_", gname[g]) + SWEPT_SUFFIX)
        swept = spec.add_mesh(name=mesh, uservert=np.concatenate(pts).ravel().tolist())
        swept.maxhullvert = -1
        spec.body(_name(m0, BODY, frame)).add_geom(
            name=mesh, type=mujoco.mjtGeom.mjGEOM_MESH, meshname=mesh, group=3, rgba=[0, 0, 0, 0])
        link_names.append(mesh)
        arm_label[mesh] = part_key(gname[g])
        replaced.append(gname[g])
    if not link_names and not root_names:
        raise SystemExit(f"no geom under {arm_root!r}: nothing to sweep")
    payload_names, record = {}, None
    if grip:
        hand = _name(m0, BODY, grip["hand"])
        for o, pose in grip["poses"].items():
            name = _unique_name(taken, f"payload_{o}")
            spec.body(hand).add_geom(name=name, type=mujoco.mjtGeom.mjGEOM_CYLINDER,
                                     size=[payload["diameter_m"] / 2.0, payload["height_m"] / 2.0, 0.0],
                                     pos=[float(v) for v in pose["pos"]], quat=pose["quat"], group=3,
                                     rgba=[0, 0, 0, 0])
            payload_names[o] = name
            arm_label[name] = payload_label(payload, o)

        def vec(v, nd=6):
            return [round(float(x), nd) + 0.0 for x in v]

        def part_of(g):
            return display_name(names, part_key(gname[g]))

        record = {"kind": payload["kind"], "diameter_m": payload["diameter_m"], "height_m": payload["height_m"],
                  "orientations": list(grip["poses"]), "hand": hand,
                  "fingers": {joint_name(m0, j): round(v, 6) for j, v in grip["holding"].items()},
                  "gap_m": round(grip["gap_m"], 6), "gap_range_m": vec(grip["gap_range_m"]),
                  "slide_axis": vec(grip["s"]), "approach_axis": vec(grip["a"]), "across_axis": vec(grip["c"]),
                  "pads": sorted({part_of(g) for g in grip["pads"]}), "pad_range_m": vec(grip["pad_range_m"]),
                  "pose": {o: {"pos_m": vec(q["pos"]), "axis": vec(q["axis"]), "quat": vec(q["quat"], 9),
                               "pushed_out_m": round(q["pushed_out_m"], 6),
                               "pushed_out_by": part_of(q["pushed_by"]) if q["pushed_by"] is not None else None}
                           for o, q in grip["poses"].items()},
                  "frame": f"the frame of the body {hand} (the fingers' body)",
                  "rule": "a cylinder held between the finger pads, swept once per orientation as an arm geom "
                          "(it meets the rover parts only), the fingers at the gap that holds it: upright = its "
                          "axis along the approach (where the fingers point: a can standing on the floor, picked "
                          "from above), across = its axis across both the slide and the approach (a can lying "
                          "down, picked from the side). Centred between the pads' inner faces and across them, "
                          "its centre at the pads' middle along the approach, pushed out along the approach "
                          "until it clears the hand's other geoms (pushed_out_m, pushed_out_by). The table "
                          "forbids a cell when the empty gripper (every jaw position) or the payload in either "
                          "orientation comes within margin_m of a rover part"}

    def compile_with(arm_on, part_margin):
        for g in spec.geoms:
            if g.name in arm_on:
                g.contype, g.conaffinity, g.margin, g.gap, g.condim = ARM_BIT, 0, 0.0, 0.0, 1
            elif g.name in label:
                g.contype, g.conaffinity, g.margin, g.gap, g.condim = 0, ARM_BIT, float(part_margin), 0.0, 1
            else:
                g.contype, g.conaffinity = 0, 0
        return spec.compile()

    # what the result needs from the first compiled model, read before it
    # goes: one compiled CAD scene holds some 350 MB
    jn = {j: joint_name(m0, j) for j in range(m0.njnt)}
    mount_names = [_name(m0, BODY, b) for b in mount_bodies]
    loose_ranges = {jn[j]: [float(v) for v in m0.jnt_range[j]] for j in loose}
    inside_geoms = {gname[g]: gname[o] for g, o in inside.items()}
    inside_parts = {display_name(names, part_key(gname[g])): display_name(names, part_key(gname[o]))
                    for g, o in inside.items()}
    holding = dict(grip["holding"]) if grip else {}
    m0 = d0 = d = None
    # the root model's parts carry the root margin itself: MuJoCo's box-box
    # contacts inside the margin band report no reliable distance, so the
    # root pass counts every contact MuJoCo makes rather than filtering on it
    model = compile_with(set(link_names), margin)
    root_model = compile_with(set(root_names), root_margin)
    payload_model = compile_with(set(payload_names.values()), margin) if payload_names else None
    gid = {geom_name(model, g): g for g in range(model.ngeom)}
    if any(geom_name(m, g) != n for m in (root_model, payload_model) if m is not None for n, g in gid.items()):
        raise SystemExit("the compiled models number their geoms differently")
    base_qpos = model.qpos0.copy()
    for n, v in fixed.items():
        base_qpos[model.jnt_qposadr[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, n)]] = v
    for j, v in open_pose.items():
        base_qpos[model.jnt_qposadr[j]] = v
    payload_qpos = base_qpos.copy()
    for j, v in holding.items():
        payload_qpos[model.jnt_qposadr[j]] = v
    part_ids = [gid[n] for n in part_names]
    keys = set(label.values()) | set(arm_label.values())
    display = {k: display_name(names, k) for k in keys}
    if lidar.get("added"):
        lidar["overlaps_m"] = lidar_overlaps(model, base_qpos, gid[LIDAR_NAME], part_ids,
                                             {gid[n]: display[label[n]] for n in part_names})
    return Prepared(model=model, root_model=root_model, root_bid=root_bid,
                    arm_ids=[gid[n] for n in link_names], root_ids=[gid[n] for n in root_names],
                    part_ids=part_ids, part_label=[label[n] for n in part_names], arm_label=arm_label,
                    display=display, lidar=lidar, stand_ins=stand_ins, base_qpos=base_qpos,
                    mount_bodies=mount_names,
                    unused=unused, loose=loose_ranges,
                    open_pose={jn[j]: round(v, 9) for j, v in open_pose.items()},
                    couplings={jn[j1]: {"follows": jn[j2], "polycoef": c} for j1, (j2, c) in couplings.items()
                               if j1 in loose},
                    coupling_ids=couplings, fixed=fixed, replaced=replaced, geometry_sha256=geometry,
                    fingers=fingers, kept_excludes=kept_excludes, visual=visual, visual_out=visual_out,
                    hull_cap_lifted=hull_cap_lifted, rover_bodies=rover_names, visual_kept=visual_kept,
                    rover_counted=len(as_is), rover_stand_in_meshes=len(meshes) if voxel > 0 else 0,
                    inside=inside_geoms, inside_parts=inside_parts,
                    payload=record, payload_model=payload_model, payload_qpos=payload_qpos,
                    payload_ids={o: gid[n] for o, n in payload_names.items()})


def held_inside(model, data, geoms) -> dict[int, int]:
    """{geom: the geom it lies in} for the geoms held whole inside another of
    `geoms` as MuJoCo collides it (a mesh as its convex hull, a primitive as
    it is). The larger geom of two is the container; a container is never
    left out itself."""
    solids = {g: _Solid(model, data, g) for g in geoms}
    order = sorted((g for g in geoms if solids[g].lo is not None),
                   key=lambda g: -float(np.prod(solids[g].hi - solids[g].lo)))
    kept, out = [], {}
    for g in order:
        s = solids[g]
        pts = _to_world(data, g, hull_points_local(model, g))
        for o in kept:
            so = solids[o]
            if np.all(s.lo >= so.lo) and np.all(s.hi <= so.hi) and so.inside(pts, 0.0).all():
                out[g] = o
                break
        else:
            kept.append(g)
    return out


def lidar_overlaps(model, base_qpos, lidar_id: int, part_ids, part_name: dict) -> dict:
    """{rover part: signed distance (m)} for the parts the lidar cylinder goes
    into (distance < 0). part_name: {rover geom id: its part's name}."""
    import mujoco

    d = mujoco.MjData(model)
    d.qpos[:] = base_qpos
    mujoco.mj_kinematics(model, d)
    flags = int(model.opt.disableflags)
    native = int(getattr(mujoco.mjtDisableBit, "mjDSBL_NATIVECCD", 0))
    out: dict[str, float] = {}
    try:
        for g in part_ids:
            if g == lidar_id:
                continue
            # libccd sees a part held whole inside the cylinder, MuJoCo's own
            # collider gives the true depth: an overlap for either, the depth
            # of the native one when it sees it
            model.opt.disableflags = flags & ~native
            deep = float(mujoco.mj_geomDistance(model, d, lidar_id, g, 0.01, None))
            model.opt.disableflags = flags | native
            held = float(mujoco.mj_geomDistance(model, d, lidar_id, g, 0.01, None))
            if min(deep, held) < 0:
                dist = deep if deep < 0 else held
                name = part_name[g]
                out[name] = round(min(dist, out.get(name, dist)), 4)
    finally:
        model.opt.disableflags = flags
    return dict(sorted(out.items(), key=lambda kv: kv[1]))


def joint_grid(model, jid: int, steps: int, pad_deg: float, travel=None) -> tuple[np.ndarray, list[float]]:
    """The joint's grid: its model range widened by pad_deg on both sides (a
    hinge), and at least `travel` ([lo, hi] radians: the calibrated travel
    and its pad, calibrated_travel); and its model range."""
    import mujoco

    if model.jnt_limited[jid]:
        lo, hi = (float(v) for v in model.jnt_range[jid])
    else:
        lo, hi = -math.pi, math.pi
    hinge = int(model.jnt_type[jid]) == int(mujoco.mjtJoint.mjJNT_HINGE)
    pad = math.radians(pad_deg) if hinge else 0.0
    if steps < 2:
        raise SystemExit("every joint needs 2 steps or more")
    a, b = lo - pad, hi + pad
    if travel is not None and hinge:
        a, b = min(a, float(travel[0])), max(b, float(travel[1]))
    return np.linspace(a, b, steps), [lo, hi]


def calibrated_travel(path, joints, joint_map: dict, pad_deg: float = TRAVEL_PAD_DEG) -> dict:
    """{joint: {"ticks": [range_min, range_max], "rad": [lo, hi], "grid_rad":
    [lo - pad, hi + pad]}} from the arm's calibration (config/arm_calibration.json:
    each joint's range in reported ticks) through the joint_map
    ({joint: {"model_zero_ticks", "sign"}})."""
    try:
        doc = json.loads(Path(path).read_text())
    except (OSError, ValueError) as exc:
        raise SystemExit(f"--calibration {path}: {exc}") from None
    out = {}
    pad = math.radians(pad_deg)
    for j in joints:
        e = doc.get(j) if isinstance(doc, dict) else None
        if not isinstance(e, dict) or "range_min" not in e or "range_max" not in e:
            raise SystemExit(f"--calibration {path}: no range_min and range_max for {j}")
        m = joint_map.get(j, {})
        z, sign = float(m.get("model_zero_ticks", MODEL_ZERO_TICKS)), int(m.get("sign", 1))
        rad = sorted(sign * (float(e[k]) - z) * 2.0 * math.pi / TICKS_PER_REV for k in ("range_min", "range_max"))
        out[j] = {"ticks": [int(e["range_min"]), int(e["range_max"])], "rad": [round(v, 9) for v in rad],
                  "grid_rad": [round(rad[0] - pad, 9), round(rad[1] + pad, 9)]}
    return out


def _contacts(model, data, is_arm: np.ndarray, is_part: np.ndarray, margin: float, ignore=frozenset()):
    """[(arm geom, rover geom, dist)] of the current contacts closer than
    margin, without the (arm geom, rover geom) pairs in `ignore`."""
    n = data.ncon
    if n == 0:
        return []
    out = []
    for (g1, g2), dd in zip(data.contact.geom[:n], data.contact.dist[:n]):
        if dd >= margin:
            continue
        if is_arm[g1] and is_part[g2]:
            pair = (int(g1), int(g2))
        elif is_arm[g2] and is_part[g1]:
            pair = (int(g2), int(g1))
        else:
            continue
        if pair not in ignore:
            out.append((pair[0], pair[1], float(dd)))
    return out


def _masks(model, arm_ids, part_ids):
    is_arm = np.zeros(model.ngeom, bool)
    is_arm[arm_ids] = True
    is_part = np.zeros(model.ngeom, bool)
    is_part[part_ids] = True
    return is_arm, is_part


def _touch(model, data, is_arm, is_part, margin, ignore=frozenset(), full=False):
    """The contacts at the pose in data (mj_kinematics + mj_collision, or a
    full mj_forward), or None when MuJoCo's collision code fails there."""
    import mujoco

    try:
        if full:
            mujoco.mj_forward(model, data)
        else:
            mujoco.mj_kinematics(model, data)
            mujoco.mj_collision(model, data)
    except mujoco.FatalError:
        return None
    return _contacts(model, data, is_arm, is_part, margin, ignore)


def _owners(grid: np.ndarray, v: float) -> list[int]:
    """The grid indices whose half-step neighbourhood holds v: a pose
    between two grid values touches both (the guard's rule), so a contact
    anywhere within half a step of a grid value must forbid that value."""
    eps = 1e-12
    out = []
    for i in range(len(grid)):
        lo = grid[0] if i == 0 else (grid[i - 1] + grid[i]) / 2.0
        hi = grid[-1] if i == len(grid) - 1 else (grid[i] + grid[i + 1]) / 2.0
        if lo - eps <= v <= hi + eps:
            out.append(i)
    return out


def root_pass(p: Prepared, joint_ids: list[int], grids: list[np.ndarray],
              step_rad: float = math.radians(ROOT_STEP_DEG), full: bool = False):
    """The root link along its own swept joints, sampled every step_rad
    (finer than the grid): (the grid axes of those joints, {their grid
    indices: the contacts found within half a grid step of that value, or
    None after a collision error}, the (root geom, rover part) pairs left
    out because the root link touches that part at every sample, the number
    of samples). Every contact MuJoCo makes counts: the root model's parts
    carry the root margin (prepare)."""
    import mujoco

    model = p.root_model
    axes = [k for k, j in enumerate(joint_ids) if int(model.jnt_bodyid[j]) == p.root_bid]
    if not p.root_ids:
        return axes, {}, frozenset(), 0
    fine = []
    for k in axes:
        g = grids[k]
        fine.append(np.unique(np.concatenate([g, np.linspace(g[0], g[-1], int(math.ceil((g[-1] - g[0]) / step_rad)) + 1)])))
    is_arm, is_part = _masks(model, p.root_ids, p.part_ids)
    raw = {}
    for combo in itertools.product(*fine):
        data = mujoco.MjData(model)
        data.qpos[:] = p.base_qpos
        for k, v in zip(axes, combo):
            data.qpos[model.jnt_qposadr[joint_ids[k]]] = v
        raw[combo] = _touch(model, data, is_arm, is_part, math.inf, full=full)
    always = None
    if all(h is not None for h in raw.values()):
        for h in raw.values():
            parts = {r for _, r, _ in h}
            always = parts if always is None else always & parts
    ignore = frozenset((a, r) for a in p.root_ids for r in (always or ()))
    merged: dict = {}
    for combo, h in raw.items():
        for idx in itertools.product(*[_owners(grids[k], v) for k, v in zip(axes, combo)]):
            if h is None or merged.get(idx, {}) is None:
                merged[idx] = None
                continue
            cell = merged.setdefault(idx, {})
            for a, r, dd in h:
                if (a, r) not in ignore:
                    cell[(a, r)] = min(dd, cell.get((a, r), dd))
    hits = {idx: (None if c is None else [(a, r, dd) for (a, r), dd in c.items()]) for idx, c in merged.items()}
    return axes, hits, ignore, len(raw)


def _sweep_range(model, joint_ids, grids, base_qpos, arm_ids, part_ids, margin, part_no, arm_no, root_axes,
                 root_hits, split, lo: int, hi: int):
    """The flat cells lo..hi-1: (lo, occupancy, part numbers, pair counts,
    part counts, collision errors, split bits: per cell the OR of split[arm
    geom] over the arm geoms in contact, every bit after a collision error)."""
    import mujoco

    data = mujoco.MjData(model)
    is_arm, is_part = _masks(model, arm_ids, part_ids)
    err_no = max(part_no.values()) + 1
    occ = np.zeros(hi - lo, np.uint8)
    cell_part = np.zeros(hi - lo, np.uint16)
    split = split or {}
    grp = np.zeros(hi - lo, np.uint8)
    every_bit = 0
    for bit in split.values():
        every_bit |= bit
    pair_cells: Counter = Counter()
    part_cells: Counter = Counter()
    errors = 0
    adr = [int(model.jnt_qposadr[j]) for j in joint_ids]
    data.qpos[:] = base_qpos
    indices = itertools.product(*[range(len(g)) for g in grids])
    for i, idx in enumerate(itertools.islice(indices, lo, hi)):
        for a, g, k in zip(adr, grids, idx):
            data.qpos[a] = g[k]
        hits = _touch(model, data, is_arm, is_part, margin) if arm_ids else []
        root = root_hits.get(tuple(idx[k] for k in root_axes), []) if root_hits else []
        if hits is None or root is None:
            errors += 1
            occ[i] = 1
            grp[i] = every_bit
            cell_part[i] = err_no
            part_cells[err_no] += 1
            data = mujoco.MjData(model)
            data.qpos[:] = base_qpos
            continue
        hits = hits + root
        if hits:
            occ[i] = 1
            per_part = Counter(part_no[r] for _, r, _ in hits)
            deepest = {}
            for _, r, dd in hits:
                deepest[part_no[r]] = min(dd, deepest.get(part_no[r], dd))
            cell_part[i] = max(per_part, key=lambda k: (per_part[k], -deepest[k], -k))
            for pair in {(arm_no.get(a, a), part_no[r]) for a, r, _ in hits}:
                pair_cells[pair] += 1
            for k in per_part:
                part_cells[k] += 1
            for a, _, _ in hits:
                grp[i] |= split.get(a, 0)
    return lo, occ, cell_part, pair_cells, part_cells, errors, grp


_JOB = None     # what a forked worker sweeps (set by sweep() before the pool starts)


def _sweep_job(bounds):
    return _sweep_range(*_JOB, *bounds)


def sweep(model, joint_ids: list[int], grids: list[np.ndarray], base_qpos: np.ndarray,
          arm_ids: list[int], part_ids: list[int], margin: float, part_no=None, root_axes=(), root_hits=None,
          arm_no=None, progress=None, jobs: int = 1, split=None):
    """The grid, row-major (the last joint fastest). part_no: {rover geom: its
    part number, 1..} (a mesh's stand-ins share one), default one per geom
    in part_ids order; arm_no likewise for the arm geoms (pair counts).
    root_hits: the root link's contacts per index of its own joints
    (root_pass), added to every cell with that index. jobs > 1 splits the
    grid over forked processes (same result). split: {arm geom: a bit} (one
    payload orientation per bit). Returns occupancy (uint8, 1 = forbidden),
    the most-hit part per cell (0 = free, the part number, or the largest
    part number + 1 for a collision error), the cell counts per (arm number,
    part number) and per part number, the number of collision errors, and
    per cell the OR of the bits of the arm geoms in contact (uint8)."""
    import multiprocessing

    global _JOB
    part_no = part_no or {g: k + 1 for k, g in enumerate(part_ids)}
    total = int(np.prod([len(g) for g in grids]))
    occ = np.zeros(total, np.uint8)
    cell_part = np.zeros(total, np.uint16)
    grp = np.zeros(total, np.uint8)
    pair_cells: Counter = Counter()
    part_cells: Counter = Counter()
    errors = 0
    size = max(200, total // (max(1, jobs) * 8))
    chunks = [(lo, min(total, lo + size)) for lo in range(0, total, size)]
    job = (model, joint_ids, grids, base_qpos, arm_ids, part_ids, margin, part_no, arm_no or {},
           tuple(root_axes), root_hits, split)
    t0, done, shown = time.monotonic(), 0, 0

    def merge(res):
        nonlocal errors, done, shown
        lo, o, c, pc, partc, e, gb = res
        occ[lo:lo + len(o)] = o
        cell_part[lo:lo + len(o)] = c
        grp[lo:lo + len(o)] = gb
        pair_cells.update(pc)
        part_cells.update(partc)
        errors += e
        done += len(o)
        if progress and done < total and done - shown >= total / 10:
            shown = done
            progress(done, total, time.monotonic() - t0)

    if jobs > 1 and len(chunks) > 1 and "fork" in multiprocessing.get_all_start_methods():
        _JOB = job
        try:
            with multiprocessing.get_context("fork").Pool(min(jobs, len(chunks))) as pool:
                for res in pool.imap_unordered(_sweep_job, chunks):
                    merge(res)
        finally:
            _JOB = None
    else:
        for lo, hi in chunks:
            merge(_sweep_range(*job, lo, hi))
    return occ, cell_part, pair_cells, part_cells, errors, grp


def _pick(occ: np.ndarray, n: int, rng, free_only: bool = False) -> list[int]:
    pools = [np.flatnonzero(occ == 0)] if free_only else [np.flatnonzero(occ), np.flatnonzero(occ == 0)]
    pick = []
    for pool in pools:
        if len(pool):
            pick.extend(rng.choice(pool, size=min(len(pool), max(1, n // len(pools))), replace=False).tolist())
    return pick


def recheck(p: Prepared, joint_ids, grids, margin, occ, n: int, seed: int = 0,
            step_rad: float = math.radians(ROOT_STEP_DEG), with_payload: bool = True) -> list[int]:
    """Grid points re-done with a full mj_forward on fresh data, the root
    link included, and the payload in every orientation when p has one (and
    with_payload): the flat indices where the verdict differs from the table
    (half the sample from the forbidden cells, half from the free ones)."""
    import mujoco

    if n <= 0:
        return []
    rng = np.random.default_rng(seed)
    shape = tuple(len(g) for g in grids)
    axes, root_hits, _, _ = root_pass(p, joint_ids, grids, step_rad, full=True)
    is_arm, is_part = _masks(p.model, p.arm_ids, p.part_ids)
    pay = getattr(p, "payload_model", None) if with_payload else None
    if pay is not None:
        pay_arm, pay_part = _masks(pay, list(p.payload_ids.values()), p.part_ids)
    bad = []
    for flat in _pick(occ, n, rng):
        idx = np.unravel_index(flat, shape)
        data = mujoco.MjData(p.model)
        data.qpos[:] = p.base_qpos
        for j, g, i in zip(joint_ids, grids, idx):
            data.qpos[p.model.jnt_qposadr[j]] = g[i]
        hits = _touch(p.model, data, is_arm, is_part, margin, full=True) if p.arm_ids else []
        root = root_hits.get(tuple(int(idx[k]) for k in axes), []) if root_hits else []
        hit = hits is None or root is None or bool(hits) or bool(root)
        if not hit and pay is not None:
            data = mujoco.MjData(pay)
            data.qpos[:] = p.payload_qpos
            for j, g, i in zip(joint_ids, grids, idx):
                data.qpos[pay.jnt_qposadr[j]] = g[i]
            held = _touch(pay, data, pay_arm, pay_part, margin, full=True)
            hit = held is None or bool(held)
        if hit != bool(occ[flat]):
            bad.append(int(flat))
    return bad


def finger_check(real: Prepared, joint_ids, grids, margin, occ, n: int, seed: int = 1) -> tuple[int, list]:
    """Free grid points of the table, done again with the real finger geoms
    closed, half open, open and at a random opening (a finger a coupling ties
    to another follows it): the (flat index, finger values) where the arm
    touches the rover. The swept volumes hold every finger position, so the
    list must be empty."""
    import mujoco

    model = real.model
    if n <= 0 or not real.loose:
        return 0, []
    rng = np.random.default_rng(seed)
    couplings = getattr(real, "coupling_ids", {})
    loose = [(mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, j), lo, hi) for j, (lo, hi) in real.loose.items()]
    followers = {j for j, _, _ in loose if j in couplings and couplings[j][0] in {k for k, _, _ in loose}}
    settings = [{j: lo for j, lo, _ in loose}, {j: (lo + hi) / 2 for j, lo, hi in loose}, {j: hi for j, _, hi in loose}]
    is_arm, is_part = _masks(model, real.arm_ids, real.part_ids)
    shape = tuple(len(g) for g in grids)
    bad, done = [], 0
    for flat in _pick(occ, n, rng, free_only=True):
        idx = np.unravel_index(flat, shape)
        for fing in settings + [{j: rng.uniform(lo, hi) for j, lo, hi in loose}]:
            data = mujoco.MjData(model)
            data.qpos[:] = real.base_qpos
            for j, g, i in zip(joint_ids, grids, idx):
                data.qpos[model.jnt_qposadr[j]] = g[i]
            for j, v in fing.items():
                data.qpos[model.jnt_qposadr[j]] = v
            apply_couplings(model, data.qpos, couplings, followers)
            hits = _touch(model, data, is_arm, is_part, margin)
            done += 1
            if hits:
                bad.append((int(flat), {joint_name(model, j): round(float(data.qpos[model.jnt_qposadr[j]]), 6)
                                        for j, _, _ in loose}))
    return done, bad


def keep_joint_map(out: Path, joints) -> tuple[dict, str]:
    """The joint_map of the table being replaced (the measured alignment) and
    its basis, else model_zero_ticks 2048 and sign +1 for every joint with
    the first alignment's basis (JOINT_MAP_BASIS)."""
    try:
        doc = json.loads(Path(out).read_text())
        old = doc.get("joint_map", {}) or {}
        basis = doc.get("joint_map_basis")
    except (OSError, ValueError, AttributeError):
        old, basis = {}, None
    jm = {}
    for j in joints:
        e = old.get(j, {}) if isinstance(old, dict) else {}
        e = e if isinstance(e, dict) else {}
        jm[j] = {"model_zero_ticks": e.get("model_zero_ticks", MODEL_ZERO_TICKS), "sign": e.get("sign", 1)}
    default = all(v == {"model_zero_ticks": MODEL_ZERO_TICKS, "sign": 1} for v in jm.values())
    if not isinstance(basis, str) or not basis:
        basis = JOINT_MAP_BASIS if default else "kept from the table this one replaced (no basis written there)"
    return jm, basis


def write_table(out: Path, doc: dict, big: dict) -> int:
    """The JSON: the header indented, each big array compact on one line."""
    head = json.dumps(doc, indent=1)
    body = ",\n".join(f' {json.dumps(k)}: {json.dumps(v, separators=(",", ":"))}' for k, v in big.items())
    text = head[:-2] + ",\n" + body + "\n}\n"
    out.parent.mkdir(parents=True, exist_ok=True)
    tmp = out.with_suffix(out.suffix + ".tmp")
    tmp.write_text(text)
    os.replace(tmp, out)
    return len(text)


def _parse_fixed(text: str) -> dict[str, float]:
    out = {}
    for item in filter(None, (s.strip() for s in (text or "").split(","))):
        name, _, value = item.partition("=")
        out[name.strip()] = float(value)
    return out



MMAP_THRESHOLD = 128 << 10    # glibc serves allocations this large and larger by mmap: freed, they go back


def _lean_malloc() -> None:
    """One compiled CAD scene holds some 350 MB and its compilation more: with
    glibc serving every allocation of 128 KB or more by mmap, what a dropped
    model held goes back to the system instead of staying in the heap."""
    try:
        import ctypes
        ctypes.CDLL("libc.so.6").mallopt(-3, MMAP_THRESHOLD)       # M_MMAP_THRESHOLD
    except (OSError, AttributeError):
        pass


def _trim() -> None:
    """Hands the memory a dropped model freed back to the system: MuJoCo's
    asset cache emptied (a scene loads as fast without it), glibc's free
    pages returned."""
    try:
        import mujoco
        mujoco.mj_clearCache(mujoco.mj_getCache())
    except (ImportError, AttributeError, TypeError):
        pass
    try:
        import ctypes
        ctypes.CDLL("libc.so.6").malloc_trim(0)
    except (OSError, AttributeError):
        pass


REUSE_KEYS = ("mujoco", "layout", "shape", "grid", "margin_m", "pad_deg", "arm_root", "rover_body", "rover_parts",
              "visual", "exclude", "excludes_kept", "lidar", "mount_parts")


def _reuse_view(doc: dict, n_arm: int) -> dict:
    """What two tables share when one reuses the other's empty-gripper sweep:
    the model by both hashes, the names, the grid, the margin, the rover's
    and the arm's settings, the fingers and the arm's parts."""
    view = {k: doc.get(k) for k in REUSE_KEYS}
    model = doc.get("model") or {}
    view["model"] = [model.get("sha256"), model.get("geometry_sha256")]
    view["names"] = (doc.get("names") or {}).get("sha256")
    view["root_link"] = {k: (doc.get("root_link") or {}).get(k) for k in ("margin_m", "step_deg", "voxel_m")}
    view["stand_ins"] = {k: (doc.get("stand_ins") or {}).get(k) for k in ("voxel_m", "cover_tol_m")}
    view["fingers"] = {k: (doc.get("fingers") or {}).get(k) for k in ("open", "fixed")}
    view["rover_inside"] = (doc.get("rover_inside") or {}).get("geoms")
    view["arm_geoms"] = list(doc.get("arm_geoms") or [])[:n_arm]
    return json.loads(json.dumps(view))      # as a JSON file reads back: lists, string keys


def reuse_empty(path: Path, view: dict, n_arm: int, key_no: dict, err_no: int, cells: int):
    """The empty gripper's sweep from an earlier table (--reuse-empty) that
    shares view with this run (_reuse_view): its occupancy
    (occupancy_empty_bits_hex in a table with a payload), the part of each
    forbidden cell, the pairs of the arm's own parts, the cells per part and
    its collision errors, numbered for this run; and its sweep's record."""
    path = Path(path)
    try:
        old = json.loads(path.read_text())
    except (OSError, ValueError) as exc:
        raise SystemExit(f"--reuse-empty {path}: {exc}") from None
    if not isinstance(old, dict) or old.get("format") != FORMAT:
        raise SystemExit(f"--reuse-empty {path}: not a keep-out table")
    theirs = _reuse_view(old, n_arm)
    differ = sorted(k for k in view if view[k] != theirs.get(k))
    if differ:
        raise SystemExit(f"--reuse-empty {path.name}: another sweep ({', '.join(differ)} differ)")
    pay = old.get("payload") or {}
    bits = old["occupancy_empty_bits_hex"] if pay else old["occupancy_bits_hex"]
    occ = np.unpackbits(np.frombuffer(bytes.fromhex(bits), np.uint8))[:cells].astype(np.uint8)
    keys = list(old.get("part_keys") or [])
    lut = np.zeros(len(keys) + 1, np.uint16)
    for i, k in enumerate(keys, start=1):
        if k != COLLISION_ERROR and k not in key_no:
            raise SystemExit(f"--reuse-empty {path.name}: part {k} is not a rover part of this model")
        lut[i] = err_no if k == COLLISION_ERROR else key_no[k]
    cell = np.asarray(old["cell_part"], np.int64)
    if len(occ) != cells or len(cell) != cells:
        raise SystemExit(f"--reuse-empty {path.name}: {len(occ)} cells, this grid has {cells}")
    cell_raw = np.where(occ > 0, lut[cell], 0).astype(np.uint16)
    if np.any((occ > 0) & (cell_raw == 0)):
        raise SystemExit(f"--reuse-empty {path.name}: a forbidden cell names no part")
    pair_cells: Counter = Counter()
    for a, k, c in old.get("pair_cells") or []:
        if int(a) < n_arm:
            pair_cells[(int(a), int(lut[int(k) + 1]))] += int(c)
    number = {name: int(lut[i]) for i, name in enumerate(old.get("part_names") or [], start=1)}
    part_cells: Counter = Counter()
    for name, c in (old.get("part_counts") or {}).items():
        if c:
            part_cells[number[name]] += int(c)
    errors = int(old.get("collision_errors", 0)) - int(pay.get("collision_errors", 0))
    src = old.get("empty_sweep") or old
    record = {k: src.get(k) for k in ("sweep_time_s", "sweep_jobs", "sweep_ms_per_pose")}
    record["reused_from"] = {"file": path.name, "sha256": sha256_of(path)}
    return occ, cell_raw, pair_cells, part_cells, errors, record


def run(args) -> int:
    import mujoco

    t_start = time.monotonic()
    model_path = Path(args.model)
    if not model_path.is_file():
        print(f"no model at {model_path}", file=sys.stderr)
        return 2
    layout = args.layout if args.layout != "auto" else layout_of(model_path)
    L = LAYOUTS[layout]
    arm_root = args.arm_root or L["arm_root"]
    rover_body = args.rover_body or L["rover_body"]
    rover_parts = args.rover_parts if args.rover_parts is not None else L["rover_parts"]
    visual = args.visual or L["visual"]
    root_voxel = args.root_voxel if args.root_voxel is not None else L["root_voxel"]
    if args.margin is None:
        args.margin = L["margin"]
    if not 0.0 <= args.root_margin <= args.margin:
        print("--root-margin goes from 0 to --margin", file=sys.stderr)
        return 2
    joints = [j.strip() for j in args.joints.split(",") if j.strip()]
    steps = [int(s) for s in args.steps.split(",")]
    if len(steps) != len(joints):
        print(f"--steps gives {len(steps)} numbers for {len(joints)} joints", file=sys.stderr)
        return 2
    parts = tuple(p.strip() for p in rover_parts.split(",") if p.strip())
    exclude = tuple(p.strip() for p in args.exclude.split(",") if p.strip())
    if args.no_lidar:
        lidar_xyz = None
    elif args.lidar_xyz:
        lidar_xyz = tuple(float(v) for v in args.lidar_xyz.split(","))
        if len(lidar_xyz) != 3:
            print("--lidar-xyz takes x,y,z in metres", file=sys.stderr)
            return 2
    elif L["lidar"]:
        lidar_xyz = profile_lidar_xyz()
    else:
        lidar_xyz = None
    payload = parse_payload(args.payload if args.payload is not None else L["payload"])
    names = load_names(args.names) if args.names else {}
    jm, basis = keep_joint_map(Path(args.out), joints)
    travel = calibrated_travel(args.calibration, joints, jm) if args.calibration else {}
    fixed = _parse_fixed(args.fix)
    jobs = args.jobs if args.jobs > 0 else min(MAX_JOBS, os.cpu_count() or 1)
    root_step = math.radians(args.root_step_deg)
    say = (lambda *a: None) if args.quiet else (lambda *a: print(*a, flush=True))
    common = dict(arm_root=arm_root, rover_body=rover_body, rover_parts=parts, exclude=exclude,
                  margin=args.margin, lidar_xyz=lidar_xyz, joints=joints, fixed=fixed,
                  voxel=args.stand_in_voxel, root_voxel=root_voxel, root_margin=args.root_margin,
                  cover_tol=args.cover_tol, visual=visual, names=names, keep_inside=args.keep_inside)
    p = prepare(model_path, **common, payload=payload)
    model = p.model
    lidar = p.lidar
    if lidar_xyz is not None and not lidar["added"]:
        lidar = {"added": False, "note": "the model has its own geom named lidar"}
    elif not lidar["added"]:
        held = sorted({p.display[k] for k in p.part_label if "lidar" in p.display[k].lower()})
        lidar = {"added": False, "note": ("the model carries the lidar as parts: " + ", ".join(held)) if held
                 else "no lidar added (--no-lidar, or a layout whose model carries it)"}
    joint_ids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, j) for j in joints]
    grids, ranges = [], []
    for j, jid, n in zip(joints, joint_ids, steps):
        g, r = joint_grid(model, jid, n, args.pad_deg, travel[j]["grid_rad"] if j in travel else None)
        grids.append(g)
        ranges.append(r)
    keys = sorted(set(p.part_label))
    if len(keys) > np.iinfo(np.uint16).max - 1:
        print(f"{len(keys)} rover parts: more than cell_part can number", file=sys.stderr)
        return 2
    shape = tuple(steps)
    total = int(np.prod(shape))
    n_stand = len(p.stand_ins["boxes"])
    n_as_is = len(p.part_ids) - n_stand
    meshes_in = sum(1 for r in p.stand_ins["per_mesh"].values() if r.get("boxes"))
    arm_keys = sorted({p.arm_label[geom_name(model, g)] for g in p.arm_ids + p.root_ids})
    orients = list(p.payload["orientations"]) if p.payload else []
    pay_ids = [p.payload_ids[o] for o in orients]
    arm_names = [p.display[k] for k in arm_keys] + [p.arm_label[geom_name(model, g)] for g in pay_ids]
    say(f"model {model_label(model_path)} (layout {layout}): {len(p.arm_ids)} link geoms "
        f"({len(p.replaced)} finger geoms as swept volumes), root link {len(p.root_ids)} geoms "
        f"({len(p.stand_ins['root_boxes'])} stand-in boxes), {len(arm_keys)} arm parts; "
        f"{n_as_is} rover geoms as they are, {n_stand} stand-in boxes for {meshes_in} rover meshes, "
        f"{len(keys)} rover parts on {', '.join(p.rover_bodies)}; visual geoms {p.visual}"
        + (f" ({p.visual_out['arm']} on the arm and {p.visual_out['rover']} on the rover left out"
           + (f"; {', '.join(p.display.get(part_key(n), n) for n in p.visual_kept)} kept: no collision geom on their body"
              if p.visual_kept else "") + ")" if p.visual == "skip" else "")
        + f"; lidar {'added at ' + str(lidar.get('pos_m')) if lidar.get('added') else 'not added'}")
    if p.kept_excludes or p.hull_cap_lifted or p.inside:
        say(f"model exclusions kept: {', '.join('|'.join(e) for e in p.kept_excludes) or 'none'}; "
            f"hull vertex cap lifted on {p.hull_cap_lifted} meshes; {len(p.inside)} rover geoms held whole inside "
            f"another left out ({len(p.inside_parts)} parts)")
    if lidar.get("overlaps_m"):
        say("warning: the lidar goes into " + ", ".join(f"{k} ({1000 * v:.1f} mm)" for k, v in lidar["overlaps_m"].items())
            + ": give its measured centre with --lidar-xyz")
    if p.loose:
        say("fingers: swept volumes over " + ", ".join(f"{j} {lo:g}..{hi:g}" for j, (lo, hi) in p.loose.items())
            + "; open (the widest, the sweep's base pose): " + ", ".join(f"{j} {v:g}" for j, v in p.open_pose.items())
            + (" (" + ", ".join(f"{j} follows {c['follows']}" for j, c in p.couplings.items()) + ")"
               if p.couplings else ""))
    if p.payload:
        r = p.payload
        say(f"payload {r['kind']} ({1000 * r['diameter_m']:g} mm across, {1000 * r['height_m']:g} mm long) in "
            f"{r['hand']}, held at a {1000 * r['gap_m']:.1f} mm gap ("
            + ", ".join(f"{j} {v:g}" for j, v in r["fingers"].items()) + f") between {', '.join(r['pads'])}; "
            + "; ".join(f"{o}: centre {[round(1000 * v, 2) for v in q['pos_m']]} mm, axis {q['axis']}"
                        + (f", pushed out {1000 * q['pushed_out_m']:.2f} mm by {q['pushed_out_by']}"
                           if q["pushed_out_by"] else "") for o, q in r["pose"].items()))
    say(f"grid {'x'.join(map(str, shape))} = {total} poses, margin {args.margin} m, pad {args.pad_deg} deg, "
        f"{jobs} job(s){', fixed: ' + ', '.join(f'{k}={v:g}' for k, v in fixed.items()) if fixed else ''}")
    if travel:
        say(f"calibrated travel (+{TRAVEL_PAD_DEG:g} deg) covered: " + ", ".join(
            f"{j} {math.degrees(t['rad'][0]):.1f}..{math.degrees(t['rad'][1]):.1f} deg in "
            f"{math.degrees(g[0]):.1f}..{math.degrees(g[-1]):.1f}" for (j, t), g in zip(travel.items(), grids)))
    key_no = {k: i + 1 for i, k in enumerate(keys)}
    part_no = {g: key_no[k] for g, k in zip(p.part_ids, p.part_label)}
    label_name = {i: p.display[k] for k, i in key_no.items()}
    label_key = {i: k for k, i in key_no.items()}
    label_name[len(keys) + 1] = COLLISION_ERROR
    label_key[len(keys) + 1] = COLLISION_ERROR
    key_of = dict(zip(p.part_ids, p.part_label))
    arm_no = {g: arm_keys.index(p.arm_label[geom_name(model, g)]) for g in p.arm_ids + p.root_ids}

    root_axes, root_hits, ignore, root_samples = root_pass(p, joint_ids, grids, root_step)
    mount = sorted({p.display[key_of[r]] for _, r in ignore})
    if mount:
        say("the root link touches these at every value of its joints (those contacts are not counted): "
            + ", ".join(mount))
    per_mesh = dict(sorted(p.stand_ins["per_mesh"].items(), key=lambda kv: -kv[1].get("outside_fraction", 0)))
    stand_keys = {part_key(m) for m in per_mesh}
    # what the table says before any sweep: the model, the grid and the settings
    head = {
        "format": FORMAT,
        "made_by": "tools/moss_arm_keepout.py",
        "model": {"path": model_label(model_path), "sha256": sha256_of(model_path), "geometry_sha256": p.geometry_sha256},
        "mujoco": mujoco.__version__,
        "layout": layout,
        "names": ({"file": Path(args.names).name, "sha256": sha256_of(Path(args.names)), "parts": len(names)}
                  if args.names else None),
        "joints": joints,
        "shape": list(shape),
        "order": "row-major: the last joint varies fastest; flat = ravel_multi_index(indices, shape)",
        "grid": {j: [round(float(v), 9) for v in g] for j, g in zip(joints, grids)},
        "model_range": {j: [round(r[0], 9), round(r[1], 9)] for j, r in zip(joints, ranges)},
        "pad_deg": args.pad_deg,
        "travel": ({"pad_deg": TRAVEL_PAD_DEG, "joints": travel,
                    "rule": "each joint's calibrated travel (the arm's range_min and range_max, reported ticks) "
                            "through joint_map, in model radians; the grid reaches pad_deg past each end"}
                   if travel else None),
        "margin_m": args.margin,
        "margin_rule": "a pose is forbidden when an arm geom comes closer than margin_m to a rover part; "
                       "MuJoCo's geom margin is set on the rover parts only",
        "arm_root": arm_root,
        "rover_body": ",".join(p.rover_bodies),
        "rover_parts": list(parts),
        "visual": visual,
        "visual_left_out": p.visual_out if visual == "skip" else {"arm": 0, "rover": 0},
        "visual_kept": {"geoms": p.visual_kept,
                        "rule": "with visual skip, a body with no collision geom keeps its visual meshes: nothing "
                                "else stands for them"},
        "exclude": list(exclude),
        "excludes_kept": p.kept_excludes,
        "hull_vertex_cap_lifted": p.hull_cap_lifted,
        "mount_bodies": p.mount_bodies,
        "arm_geoms": arm_names,
        "rover_geoms": sorted({p.display[k] for k in set(p.part_label) - stand_keys}),
        "rover_parts_total": len(keys),
        "rover_geoms_counted": p.rover_counted,
        "rover_stand_in_meshes": p.rover_stand_in_meshes,
        "rover_geoms_rule": "every collision geom of the rover bodies (not the arm's, not its mount's) is one of: "
                            "rover_geoms_counted (as it is), rover_stand_in_meshes (through stand-ins), "
                            "rover_inside.geoms (held inside another), rover_left_out (by name)",
        "rover_left_out": p.unused,
        "rover_inside": {"geoms": len(p.inside), "parts": p.inside_parts,
                         "rule": "a rover geom held whole inside another (as MuJoCo collides it) adds no forbidden "
                                 "pose: an arm geom near it is nearer the other; left out (--keep-inside keeps it)"},
        "stand_ins": {"voxel_m": p.stand_ins["voxel_m"], "cover_tol_m": p.stand_ins["cover_tol_m"], "boxes": n_stand,
                      "rule": "per rover mesh: the share of its sampled surface lying more than cover_tol_m outside "
                              "the parts that count as they are, the extent of that share in the rover frame (m), "
                              "and the boxes standing in for it",
                      "per_mesh": per_mesh},
        "root_link": {"body": arm_root, "joints": [joints[k] for k in root_axes], "margin_m": args.root_margin,
                      "step_deg": args.root_step_deg, "samples": root_samples,
                      "voxel_m": p.stand_ins["root_voxel_m"], "stand_in_boxes": p.stand_ins["root"],
                      "rule": "its own joints sampled every step_deg, its meshes as surface stand-ins when voxel_m "
                              "is above 0 (else its geoms as they are); a contact closer than margin_m forbids "
                              "every grid value within half a grid step"},
        "mount_parts": mount,
        "mount_rule": "the root link touches these at every value of its joints: those contacts are not counted",
        "lidar": lidar,
        "fingers": {"joints": p.loose, "open": p.open_pose, "coupled": p.couplings, "fixed": p.fixed,
                    "rule": "every finger position: each geom a finger joint moves is replaced by its swept volume "
                            "(the convex hull of the geom at both ends of a slide, every 10 degrees of a hinge); "
                            "the sweep's base pose holds the fingers open: each at the end of its range that "
                            "spreads the gripper the most, a joint a model equality ties to another following it"},
        "unswept_joints": {"swept_volume": p.loose, "fixed": p.fixed, "replaced_geoms": p.replaced},
    }

    def progress(done, tot, dt):
        say(f"  {done}/{tot} poses, {dt:.0f} s, about {dt / done * (tot - done):.0f} s left")

    # the empty gripper: swept here, or taken from an earlier table of the same sweep
    t0 = time.monotonic()
    if args.reuse_empty:
        occ0, cell0, pairs0, parts0, err0, empty_rec = reuse_empty(
            Path(args.reuse_empty), _reuse_view(head, len(arm_keys)), len(arm_keys), key_no, len(keys) + 1, total)
        empty_s = 0.0
        say(f"empty gripper: {int(occ0.sum())} forbidden cells taken from {Path(args.reuse_empty).name} "
            "(the same model, grid and settings)")
    else:
        occ0, cell0, pairs0, parts0, err0, _ = sweep(model, joint_ids, grids, p.base_qpos, p.arm_ids, p.part_ids,
                                                     args.margin, part_no, root_axes, root_hits, arm_no,
                                                     progress=None if args.quiet else progress, jobs=jobs)
        empty_s = time.monotonic() - t0
        empty_rec = {"sweep_time_s": round(empty_s, 1), "sweep_jobs": jobs,
                     "sweep_ms_per_pose": round(1000.0 * empty_s * jobs / total, 3), "reused_from": None}
    # the payload: its cylinders alone (the arm at the holding gap lies inside the jaws' swept volumes)
    occ, cell_raw, pair_cells, part_cells, errors = occ0, cell0, Counter(pairs0), parts0, err0
    pay_s, pay_parts, pay_err, grp = 0.0, Counter(), 0, np.zeros(total, np.uint8)
    if pay_ids:
        say(f"payload sweep: the {payload['kind']} alone, held {' and '.join(orients)}")
        t1 = time.monotonic()
        occ_p, cell_p, pairs_p, pay_parts, pay_err, grp = sweep(
            p.payload_model, joint_ids, grids, p.payload_qpos, pay_ids, p.part_ids, args.margin, part_no,
            arm_no={g: len(arm_keys) + i for i, g in enumerate(pay_ids)},
            progress=None if args.quiet else progress, jobs=jobs, split={g: 1 << i for i, g in enumerate(pay_ids)})
        pay_s = time.monotonic() - t1
        occ = occ0 | occ_p
        cell_raw = np.where(occ0 > 0, cell0, cell_p).astype(np.uint16)
        pair_cells = pair_cells + pairs_p
        errors = err0 + pay_err
    sweep_s = empty_s + pay_s
    bad = recheck(p, joint_ids, grids, args.margin, occ0, args.check, step_rad=root_step, with_payload=False)
    if not bad and pay_ids:
        bad = recheck(p, joint_ids, grids, args.margin, occ, args.check, seed=2, step_rad=root_step)
    if bad:
        print(f"{len(bad)} grid point(s) differ between mj_kinematics+mj_collision and a full mj_forward "
              f"(first flat index {bad[0]}): table not written", file=sys.stderr)
        return 3
    p.model = p.root_model = p.payload_model = model = None      # the finger check compiles its own
    _trim()
    real = prepare(model_path, **common, fingers="real", stand_ins=p.stand_ins) if p.loose else None
    checked, touched = finger_check(real, joint_ids, grids, args.margin, occ, args.check) if real else (0, [])
    real = None
    _trim()
    if touched:
        print(f"{len(touched)} free grid point(s) touch the rover with the real fingers "
              f"(first: flat {touched[0][0]}, fingers {touched[0][1]}): table not written", file=sys.stderr)
        return 3

    forbidden = int(occ.sum())
    # the parts hit, numbered 1.. from the most frequent most-hit part down, so
    # the common ones take one digit in cell_part; the parts never hit are not listed
    top = Counter(int(v) for v in cell_raw[occ.astype(bool)])
    order = sorted(set(top) | set(part_cells) | set(pay_parts),
                   key=lambda k: (-top.get(k, 0), -part_cells.get(k, 0), -pay_parts.get(k, 0), label_name[k]))
    lut = np.zeros(len(keys) + 2, np.uint16)
    for new_no, k in enumerate(order, start=1):
        lut[k] = new_no
    cell_part = lut[cell_raw]
    # one byte per cell while the table names 255 parts or fewer
    dtype = np.uint8 if len(order) <= MAX_PARTS_U8 else np.uint16
    cell_part = cell_part.astype(dtype)
    pairs = sorted(((arm_names[a], label_name[k], c) for (a, k), c in pair_cells.items()),
                   key=lambda t: (-t[2], t[0], t[1]))
    pair_rows = sorted(([a, int(lut[k]) - 1, c] for (a, k), c in pair_cells.items()), key=lambda t: (-t[2], t[0], t[1]))
    record = None
    if pay_ids:
        empty_b = occ0.astype(bool)
        bits = {o: ((grp >> i) & 1).astype(bool) for i, o in enumerate(orients)}
        record = dict(p.payload)
        record["forbidden"] = {"empty": int(empty_b.sum()), **{o: int(b.sum()) for o, b in bits.items()},
                               "union": forbidden}
        record["added"] = {**{o: int((b & ~empty_b).sum()) for o, b in bits.items()},
                           "total": forbidden - int(empty_b.sum())}
        record["part_counts"] = {label_name[k]: pay_parts[k] for k in order if pay_parts.get(k)}
        record["collision_errors"] = pay_err
        record["sweep_time_s"] = round(pay_s, 1)
        record["sweep_ms_per_pose"] = round(1000.0 * pay_s * jobs / total, 3)
        record["counts_rule"] = ("forbidden: the cells where the empty gripper, the payload held each way, and any "
                                 "of them (union, the table) come within margin_m of a rover part; added: those of "
                                 "them the empty gripper leaves free; part_counts: the cells where the payload, "
                                 "either way, touches that part")
    run_s = time.monotonic() - t_start
    doc = dict(head)
    doc.update({
        "payload": record,
        "empty_sweep": empty_rec,
        "collision_errors": errors,
        "cells": total,
        "forbidden": forbidden,
        "forbidden_fraction": round(forbidden / total, 6),
        "part_names": [label_name[k] for k in order],
        "part_keys": [label_key[k] for k in order],
        "part_counts": {label_name[k]: part_cells[k] for k in order if part_cells.get(k)},
        "part_counts_rule": "cells where the arm, the gripper empty, touches this part (a cell counts for every "
                            "part it touches); payload.part_counts the same for the payload",
        "part_top_counts": {label_name[k]: top.get(k, 0) for k in order},
        "pair_counts": {f"{a}|{r}": c for a, r, c in pairs[:TOP_PAIRS]},
        "pair_counts_rule": f"cells where this arm part and this rover part touch: the {TOP_PAIRS} most frequent "
                            "pairs; pair_cells holds every pair as [i, k, cells] for arm_geoms[i] and part_names[k]",
        "pairs": len(pairs),
        "run_time_s": round(run_s, 1),
        "sweep_time_s": round(sweep_s, 1),
        "sweep_jobs": jobs,
        "sweep_ms_per_pose": empty_rec.get("sweep_ms_per_pose"),
        "rechecked_with_mj_forward": args.check,
        "finger_check": {"poses": checked, "touching": len(touched)},
        "ticks_per_rev": TICKS_PER_REV,
        "joint_map": jm,
        "joint_map_basis": basis,
        "joint_map_rule": "rad = sign * (ticks - model_zero_ticks) * 2 pi / ticks_per_rev; "
                          "ticks as the servo reports them (homing offset applied); align with two poses (docs/arm.md)",
        "occupancy_rule": "occupancy: one uint8 per cell, 1 = forbidden, flat in row-major order, packed 8 cells "
                          "per byte (numpy packbits, first cell in the high bit), written in hex; with a payload, "
                          "occupancy_empty_bits_hex holds the empty gripper's alone, the same way",
        "cell_part_rule": "0 = free, k = part_names[k - 1]: the rover part with the most contacts in that cell "
                          "(the empty gripper's where it forbids the cell, else the payload's)",
        "cell_part_dtype": np.dtype(dtype).name,
        "occupancy_bits_hex": np.packbits(occ).tobytes().hex(),
    })
    if record is not None:
        doc["occupancy_empty_bits_hex"] = np.packbits(occ0).tobytes().hex()
    size = write_table(Path(args.out), doc, {"pair_cells": pair_rows, "cell_part": cell_part.tolist()})

    say(f"{total} cells, {forbidden} forbidden ({100.0 * forbidden / total:.1f} %), "
        + (f"empty gripper taken from {empty_rec['reused_from']['file']}" if empty_rec["reused_from"]
           else f"sweep {empty_s:.1f} s ({1000.0 * empty_s * jobs / total:.2f} ms per pose and job)")
        + (f", payload sweep {pay_s:.1f} s" if pay_ids else "")
        + f", total {run_s:.1f} s, {size / 1e6:.2f} MB -> {args.out}")
    if record is not None:
        f, a = record["forbidden"], record["added"]
        say(f"payload: empty gripper {f['empty']} cells, " + ", ".join(
            f"{o} {f[o]} ({a[o]} the empty gripper leaves free)" for o in orients) + f", union {f['union']}")
    if errors:
        say(f"warning: {errors} pose(s) where MuJoCo's collision failed, marked forbidden")
    n_hit = sum(1 for k in order if k <= len(keys))
    say(f"rover parts hit: {n_hit} of {len(keys)}{' (and the collision errors)' if errors else ''}, "
        f"cell_part {np.dtype(dtype).name}")
    say("rover parts hit (cells): " + ", ".join(f"{label_name[k]} {part_cells.get(k, 0) or pay_parts.get(k, 0)}"
                                               for k in order[:12]))
    say("worst pairs (cells): " + ", ".join(f"{a}|{r} {c}" for a, r, c in pairs[:8]))
    if checked:
        say(f"finger check: {checked} poses of free grid points with the real fingers, none touches")
    return 0


def main(argv=None) -> int:
    _lean_malloc()
    ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
    ap.add_argument("--model", required=True, help="the robot's MJCF")
    ap.add_argument("--out", default=str(DEFAULT_OUT), help="the table (default config/arm_keepout.json)")
    ap.add_argument("--layout", default="auto", choices=("auto", "cad", "sim"),
                    help="the defaults below: cad for the CAD scene (a body rover_exterior), sim otherwise")
    ap.add_argument("--names", default="", help="a parts manifest: {\"parts\": [{\"id\", \"name\"}]} names the parts")
    ap.add_argument("--steps", default=",".join(map(str, STEPS)), help="grid points per joint, in --joints order")
    ap.add_argument("--margin", type=float, default=None,
                    help="metres: closer than this is a contact (layout: 0.01; 0.002)")
    ap.add_argument("--pad-deg", type=float, default=PAD_DEG, help="each hinge range widened by this on both sides")
    ap.add_argument("--calibration", default="",
                    help="the arm's calibration (config/arm_calibration.json): the grid covers each joint's "
                         f"calibrated travel through the joint_map, plus {TRAVEL_PAD_DEG:g} degrees")
    ap.add_argument("--joints", default=",".join(JOINTS))
    ap.add_argument("--arm-root", default=None, help="the first moving body of the arm (layout: shoulder_link, shoulder)")
    ap.add_argument("--rover-body", default=None,
                    help="the rover's bodies, comma separated (layout: rover; rover_exterior,base)")
    ap.add_argument("--rover-parts", default=None,
                    help="geom name patterns (fnmatch) that count as they are, meshes included "
                         "(layout: hull,bin_*,track_*,tread_*,lidar; *)")
    ap.add_argument("--visual", default=None, choices=("count", "skip"),
                    help="the geoms the model makes visual only (contype and conaffinity 0): parts, or left out "
                         "(layout: count; skip)")
    ap.add_argument("--exclude", default="", help="geom name patterns (fnmatch) that are not rover parts")
    ap.add_argument("--keep-inside", action="store_true",
                    help="keep the rover geoms held whole inside another one (they add no forbidden pose)")
    ap.add_argument("--stand-in-voxel", type=float, default=VOXEL_M,
                    help="metres: the box grid of the rover meshes' stand-ins (0: those meshes are left out)")
    ap.add_argument("--root-voxel", type=float, default=None,
                    help="metres: the box grid of the root link's stand-ins, 0: its geoms as they are "
                         "(layout: 0.001; 0)")
    ap.add_argument("--root-margin", type=float, default=ROOT_MARGIN_M,
                    help="metres: the root link's contacts count closer than this (it turns in place; at most --margin)")
    ap.add_argument("--root-step-deg", type=float, default=ROOT_STEP_DEG,
                    help="degrees between the root link's samples along its own joints")
    ap.add_argument("--cover-tol", type=float, default=COVER_TOL_M,
                    help="metres: a mesh surface this close to a part that counts as it is needs no stand-in")
    ap.add_argument("--lidar-xyz", default="", help="x,y,z of the lidar's centre on the first rover body (m)")
    ap.add_argument("--no-lidar", action="store_true", help="do not add the lidar")
    ap.add_argument("--fix", default="", help="name=value,... pins unswept arm joints (else swept as volumes)")
    ap.add_argument("--payload", default=None,
                    help="what the gripper holds, swept upright and across: none, can (66 x 120 mm) or "
                         "cyl:<diameter mm>x<height mm> (layout: none; can)")
    ap.add_argument("--reuse-empty", default="",
                    help="an earlier table of the same model, grid and settings: its empty-gripper sweep is taken "
                         "as it is and only the payload is swept")
    ap.add_argument("--check", type=int, default=CHECK_N,
                    help="grid points re-done with mj_forward, and free ones with the real fingers")
    ap.add_argument("--jobs", type=int, default=0, help="processes for the sweep (0: one per core, up to 16)")
    ap.add_argument("--quiet", action="store_true")
    try:
        import mujoco  # noqa: F401
    except ImportError:
        print("mujoco is not installed: pip install mujoco", file=sys.stderr)
        return 2
    args = ap.parse_args(argv)
    if (args.root_voxel is not None and args.root_voxel < 0) or args.root_step_deg <= 0:
        print("--root-voxel must be 0 or more and --root-step-deg positive", file=sys.stderr)
        return 2
    try:
        return run(args)
    finally:
        import gc
        gc.collect()
        _trim()                         # the run's models are gone: hand their memory back


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