Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python

arm drop: the pick-and-drop sequence as numbers (config/arm_drop_poses.json), tools/moss_arm_drop.py plan checks every move against the keep-out guard and the CAD scene, cold bench

metroxf8bf35e4e88f
diff --git a/benches/bench_arm_drop_cold.py b/benches/bench_arm_drop_cold.py
new file mode 100644
index 0000000..205bdc1
--- /dev/null
+++ b/benches/bench_arm_drop_cold.py
@@ -0,0 +1,352 @@
+"""Cold bench for tools/moss_arm_drop.py plan: the pick-and-drop sequence of
+config/arm_drop_poses.json checked against the keep-out guard, with nothing
+connected. numpy; mujoco for G (skipped without it, or without the CAD scene).
+
+Known input -> known output, in ticks, degrees, millimetres and newton metres:
+
+ A. the numbers: every pose's ticks are the table's joint_map on its angles
+ (travel 1.357553 rad on the pan -> 2933 ticks; pick's pan 76.78 deg ->
+ 2922 ticks -> 76.77 deg, half a tick); the gripper: 90 mm -> 3674 ticks,
+ 53 mm -> 2780, 71.5 mm -> 3227; the quintic: 0 at 0, 0.5 at 0.5, 1 at 1,
+ its peak speed 1.875 times the mean at the middle; a move cut into
+ samples a quarter of a grid step apart at most, and the same move the
+ other way is exactly its samples reversed
+ B. the order: drive, travel, pick (the gripper closes), lift, clear, drop
+ (the gripper opens), clear, drive; the pan from clear to drop turns
+ through 0 (the angles are never wrapped): every sample of the transfer
+ has its pan between clear's and drop's, in order
+ C. plan on the sequence as configured (the proposal applied), clearance 1:
+ exit 0, every move OK; checked again here on every sample, with the
+ table's forbidden points grown by one grid step by this bench: each
+ sample allowed and inside the calibrated ranges, each step between two
+ samples through segment_allowed and a box check (every grid point
+ between the two samples, each widened by 25 ticks, free)
+ D. plan --as-delivered: refused (exit 1); at clearance 0 the delivered travel
+ is refused on the arm's own base and the delivered drop on the bin, pick,
+ lift and clear pass. The proposal reproduces: the smallest shift for the
+ delivered drop at clearance 1 is the configured one (shoulder_lift -9,
+ wrist_flex -10 deg)
+ E. the way back is the way out reversed: the move from drop back to clear
+ passes through exactly the samples of the move from clear to drop, in the
+ other order
+ F. a pose inside the rover (a forbidden grid point of the table whose
+ neighbours are all forbidden) put in place of drop is refused by plan,
+ and why() names the part; a speed over 400 ticks/s without clearance is
+ refused; no calibration, no plan
+ G. the CAD scene (MOSS_ARM_CAD, else ~/moss-*/cad/*/MOSS_R31.xml): every
+ delivered pose's tool point lands on its tcp_m within 1 mm; plan --mujoco
+ counts 0 contacts on the whole sequence; the poses to wait in (drive and
+ clear) hold the tool point within 12 cm of the pan axis and load the
+ shoulder with 0.2 N m or less, the pick (the arm reaching to the floor)
+ more than 15 cm out and more than 0.5 N m; the process stays under 4 GB
+
+Run: PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=. python3 benches/bench_arm_drop_cold.py
+"""
+import atexit
+import copy
+import io
+import json
+import math
+import os
+import resource
+import sys
+import tempfile
+from contextlib import redirect_stdout
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "tools"))
+
+import moss_arm_drop as D # noqa: E402
+import moss_arm_sweep as W # noqa: E402
+from moss_dimos.arm_guard import ArmGuard # noqa: E402
+
+OK = 0
+KO = 0
+
+
+def check(label, ok, detail=""):
+ global OK, KO
+ print(f" {'OK ' if ok else 'KO '} {label}{' - ' + detail if detail else ''}")
+ if ok:
+ OK += 1
+ else:
+ KO += 1
+
+
+_TMP = tempfile.TemporaryDirectory(prefix="arm_drop_")
+atexit.register(_TMP.cleanup)
+TMP = Path(_TMP.name)
+TOL = 25
+JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll")
+# the drive pose of docs/arm.md, the travel of an SO-101 arm as its calibration reads (as the sweep's bench)
+FIXTURE = {
+ "shoulder_pan": {"id": 1, "range_min": 702, "range_max": 3394},
+ "shoulder_lift": {"id": 2, "homing_offset": -1369, "range_min": 860, "range_max": 3236},
+ "elbow_flex": {"id": 3, "range_min": 890, "range_max": 3206},
+ "wrist_flex": {"id": 4, "range_min": 850, "range_max": 3202},
+ "wrist_roll": {"id": 5, "range_min": 171, "range_max": 3925},
+ "gripper": {"id": 6, "range_min": 1500, "range_max": 4095},
+ "poses": {"drive": {"ticks": {"shoulder_pan": 2857, "shoulder_lift": 860, "elbow_flex": 3206, "wrist_flex": 868,
+ "wrist_roll": 3221, "gripper": 3674}}},
+}
+
+
+def quiet(fn, *args, **kw):
+ buf = io.StringIO()
+ with redirect_stdout(buf):
+ out = fn(*args, **kw)
+ return out, buf.getvalue()
+
+
+def plan(*extra):
+ """tools/moss_arm_drop.py plan, its exit status and its text."""
+ lines = []
+ code = D.main(["plan", "--table", str(table_path), "--calib", str(calib_path), *extra], say=lines.append)
+ return code, "\n".join(lines)
+
+
+def grown(occ, steps):
+ """The forbidden grid points grown by `steps` grid steps along each joint."""
+ for _ in range(steps):
+ g = occ.copy()
+ for k in range(occ.ndim):
+ lo = tuple(slice(0, -1) if i == k else slice(None) for i in range(occ.ndim))
+ hi = tuple(slice(1, None) if i == k else slice(None) for i in range(occ.ndim))
+ g[lo] |= occ[hi]
+ g[hi] |= occ[lo]
+ occ = g
+ return occ
+
+
+def touched(g, lo, hi):
+ step = g[1] - g[0]
+ flo, fhi = (lo - g[0]) / step, (hi - g[0]) / step
+ if flo < -1e-6 or fhi > len(g) - 1 + 1e-6:
+ return None
+ a = int(round(flo)) if abs(flo - round(flo)) <= 1e-6 else int(math.floor(flo))
+ b = int(round(fhi)) if abs(fhi - round(fhi)) <= 1e-6 else int(math.floor(fhi)) + 1
+ return np.arange(a, b + 1)
+
+
+def box_ok(guard, occ, a, b, drive):
+ """Every grid point between two poses (ticks), each end widened by the
+ tolerance unless it is the drive pose, is free in occ."""
+ jm, idx = guard.joint_map, []
+ pad = TOL * 2 * math.pi / 4096
+ for j, g in zip(guard.joints, guard.grids):
+ qa, qb = jm.ticks_to_rad(j, a[j]), jm.ticks_to_rad(j, b[j])
+ pa = 0.0 if a == drive else pad
+ pb = 0.0 if b == drive else pad
+ t = touched(np.asarray(g), min(qa - pa, qb - pb), max(qa + pa, qb + pb))
+ if t is None:
+ return False
+ idx.append(t)
+ return not occ[np.ix_(*idx)].any()
+
+
+# ── inputs ──
+
+table_path = Path(os.environ.get("MOSS_ARM_TABLE", ROOT / "config" / "arm_keepout.json"))
+poses_path = ROOT / "config" / "arm_drop_poses.json"
+calib_env = os.environ.get("MOSS_ARM_CALIB")
+calib_path = Path(calib_env) if calib_env else ROOT / "config" / "arm_calibration.json"
+if calib_path.is_file():
+ CAL = json.loads(calib_path.read_text())
+ cal_src = str(calib_path)
+else:
+ CAL = FIXTURE
+ calib_path = TMP / "calibration.json"
+ calib_path.write_text(json.dumps(FIXTURE))
+ cal_src = "the fixture (the drive pose of docs/arm.md)"
+GUARD = ArmGuard.load(table_path, calib_path)
+DOC = json.loads(poses_path.read_text())
+DRIVE = {j: int(CAL["poses"]["drive"]["ticks"][j]) for j in JOINTS}
+print(f"table {table_path.name}: {GUARD.summary()}")
+print(f"calibration: {cal_src}")
+
+# ── A. the numbers ──
+print("A. the numbers")
+jm = GUARD.joint_map
+bad = [(n, j) for n, p in DOC["poses"].items() for j, r, t in zip(JOINTS, p["rad"], p["ticks"]) if jm.rad_to_ticks(j, r) != t]
+check("every pose's ticks are the table's joint_map on its angles", not bad and len(DOC["poses"]) == 5, str(bad))
+t = jm.rad_to_ticks("shoulder_pan", DOC["poses"]["travel"]["rad"][0])
+check("travel's pan: 1.357553 rad -> 2933 ticks", abs(DOC["poses"]["travel"]["rad"][0] - 1.357553) < 1e-6 and t == 2933, str(t))
+deg = math.degrees(DOC["poses"]["pick"]["rad"][0])
+back = math.degrees(jm.ticks_to_rad("shoulder_pan", jm.rad_to_ticks("shoulder_pan", math.radians(deg))))
+check("pick's pan: 76.78 deg -> 2922 ticks -> back within half a tick (0.044 deg)",
+ jm.rad_to_ticks("shoulder_pan", math.radians(deg)) == 2922 and abs(back - deg) <= 360 / 4096 / 2,
+ f"{deg:.2f} -> {back:.3f} deg")
+G = DOC["gripper"]
+gt = [D.gap_to_ticks(G, mm) for mm in (90, 53, 71.5)]
+check("gripper: 90 mm -> 3674 ticks, 53 mm -> 2780, 71.5 mm -> 3227", gt == [3674, 2780, 3227], str(gt))
+du = 1e-6
+peak = (D.quintic(0.5 + du) - D.quintic(0.5 - du)) / (2 * du)
+check("quintic: 0, 0.5, 1 at 0, 0.5, 1; its peak 1.875 times the mean speed at the middle",
+ D.quintic(0) == 0 and abs(D.quintic(0.5) - 0.5) < 1e-12 and D.quintic(1) == 1 and abs(peak - 1.875) < 1e-6,
+ f"{peak:.6f}")
+STEP = {j: abs(g[1] - g[0]) * 4096 / (2 * math.pi) for j, g in zip(GUARD.joints, GUARD.grids)}
+a, b = {j: 1000 for j in JOINTS}, {"shoulder_pan": 3000, "shoulder_lift": 1001, "elbow_flex": 1500, "wrist_flex": 999, "wrist_roll": 1000}
+s = D.samples(a, b, JOINTS, STEP)
+gaps = max(abs(q[j] - p[j]) / STEP[j] for p, q in zip(s, s[1:]) for j in JOINTS)
+check("a move of 2000 ticks on the pan cut a quarter of a grid step apart at most, both ends kept",
+ s[0] == a and s[-1] == b and gaps <= 0.25 + 1e-9 and len(s) == math.ceil(2000 / (STEP["shoulder_pan"] // 4)) + 1,
+ f"{len(s)} samples, largest {gaps:.3f} grid step")
+check("the same move the other way: exactly its samples reversed", D.samples(b, a, JOINTS, STEP) == s[::-1])
+
+# ── B. the order ──
+print("B. the order")
+moves = D.build(DOC, jm, JOINTS, DRIVE)
+order = [moves[0].a.name] + [m.b.name for m in moves]
+named = [n for n in order if not n.startswith("via")]
+dedup = [n for i, n in enumerate(named) if i == 0 or n != named[i - 1]]
+check("drive, travel, pick, lift, clear, drop, clear, drive", dedup == ["drive", "travel", "pick", "lift", "clear", "drop", "clear", "drive"],
+ " ".join(dedup))
+grip = [(m.phase, m.gap_mm) for m in moves if m.gap_mm[0] != m.gap_mm[1]]
+check("the gripper closes at pick (90 -> 53 mm) and opens at drop (53 -> 90 mm), nowhere else",
+ grip == [("grasp", (90.0, 53.0)), ("release", (53.0, 90.0))]
+ and [m.a.name for m in moves if m.gap_mm[0] != m.gap_mm[1]] == ["pick", "drop"], str(grip))
+held = [m.phase for m in moves if m.holds_can]
+check("the can is held from the lift to the release", sorted(set(held)) == ["clear", "lift", "release", "transfer"], str(sorted(set(held))))
+tr = [m for m in moves if m.phase == "transfer"]
+pans = [p["shoulder_pan"] for m in tr for p in D.samples(m.a.ticks, m.b.ticks, JOINTS, STEP)]
+c_pan, d_pan = tr[0].a.ticks["shoulder_pan"], tr[-1].b.ticks["shoulder_pan"]
+check("clear -> drop: the pan turns through 0 deg (2048 ticks), never wrapped: every sample between clear's and drop's, in order",
+ all(x >= y for x, y in zip(pans, pans[1:])) and pans[0] == c_pan and pans[-1] == d_pan and c_pan > 2048 > d_pan,
+ f"{math.degrees(jm.ticks_to_rad('shoulder_pan', c_pan)):+.1f} -> {math.degrees(jm.ticks_to_rad('shoulder_pan', d_pan)):+.1f} deg, {len(pans)} samples")
+
+# ── C. the plan ──
+print("C. plan, clearance 1")
+code, text = plan()
+check("plan: exit 0, every move OK", code == 0 and "every move passes" in text and "REFUSED" not in text,
+ [ln for ln in text.splitlines() if ln.startswith("sequence:")][0][:120])
+OCC1 = grown(GUARD.occ, 1)
+G1 = copy.copy(GUARD)
+G1.occ = OCC1
+pts = []
+for m in moves:
+ pts.extend((m.phase, p) for p in D.samples(m.a.ticks, m.b.ticks, JOINTS, STEP))
+check("every sample allowed with the forbidden points grown by one step (by this bench)",
+ all(G1.allowed_ticks(p) for _, p in pts), f"{len(pts)} samples")
+check("every sample inside the calibrated ranges",
+ all(CAL[j]["range_min"] <= p[j] <= CAL[j]["range_max"] for _, p in pts for j in JOINTS))
+steps = [(p, q) for m in moves for p, q in zip(*(lambda s: (s, s[1:]))(D.samples(m.a.ticks, m.b.ticks, JOINTS, STEP)))]
+check("every step between two samples: segment_allowed", all(G1.segment_allowed(G1.ticks_pose_to_rad(p), G1.ticks_pose_to_rad(q)) for p, q in steps),
+ f"{len(steps)} steps")
+check("every step between two samples: the box, both ends widened by 25 ticks, holds free grid points only",
+ all(box_ok(GUARD, OCC1, p, q, DRIVE) for p, q in steps))
+check("no joint moves more than a quarter of a grid step between two samples",
+ max(abs(q[j] - p[j]) / STEP[j] for p, q in steps for j in JOINTS) <= 0.25 + 1e-9)
+
+# ── D. as delivered ──
+print("D. the poses as delivered")
+code, text = plan("--as-delivered")
+check("--as-delivered, clearance 1: refused (exit 1)", code == 1 and "REFUSED" in text,
+ [ln for ln in text.splitlines() if ln.startswith("sequence:")][0][:100])
+delivered = {n: dict(zip(JOINTS, p["ticks"])) for n, p in DOC["poses"].items()}
+verdict = {n: GUARD.allowed_ticks(t) for n, t in delivered.items()}
+check("clearance 0: travel and drop refused, pick, lift and clear allowed",
+ verdict == {"travel": False, "pick": True, "lift": True, "clear": True, "drop": False}, str(verdict))
+check("travel touches the arm's own base, drop the bin",
+ "SO101 base link" in GUARD.why_ticks(delivered["travel"]) and "bin" in GUARD.why_ticks(delivered["drop"]),
+ GUARD.why_ticks(delivered["drop"])[:80])
+C1 = D.Checker(GUARD, CAL, TOL, 1)
+sh = D.suggest_pose(C1, delivered["drop"])
+check("the smallest shift for the delivered drop at clearance 1 is the configured one",
+ sh == {"shoulder_lift": -9, "wrist_flex": -10} == DOC["poses"]["drop"]["shift_deg"], str(sh))
+
+# ── E. the way back ──
+print("E. the way back")
+out = [p for m in moves if m.phase == "transfer" for p in D.samples(m.a.ticks, m.b.ticks, JOINTS, STEP)]
+back = [p for m in moves if m.phase == "back_to_clear" for p in D.samples(m.a.ticks, m.b.ticks, JOINTS, STEP)]
+check("drop -> clear passes through exactly the samples of clear -> drop, in the other order",
+ back == out[::-1] and len(out) > 20, f"{len(out)} samples")
+
+# ── F. refusals ──
+print("F. refusals")
+occ = GUARD.occ
+deep = None
+for c in np.argwhere(occ):
+ sl = tuple(slice(max(x - 1, 0), x + 2) for x in c)
+ if occ[sl].all() and all(0 < x < n - 1 for x, n in zip(c, occ.shape)):
+ q = {j: float(g[c[k]]) for k, (j, g) in enumerate(zip(GUARD.joints, GUARD.grids))}
+ t = jm.pose_to_ticks(q)
+ if all(CAL[j]["range_min"] <= t[j] <= CAL[j]["range_max"] for j in JOINTS):
+ deep = q
+ break
+bad_doc = json.loads(json.dumps(DOC))
+bad_doc["poses"]["drop"]["rad"] = [deep[j] for j in JOINTS]
+bad_doc["poses"]["drop"].pop("shift_deg", None)
+for e in bad_doc["sequence"]:
+ if e["to"] == "drop" or e["from"] == "drop":
+ e.pop("via", None)
+bad_path = TMP / "inside.json"
+bad_path.write_text(json.dumps(bad_doc))
+code, text = plan("--poses", str(bad_path))
+part = GUARD.why_ticks(jm.pose_to_ticks(deep))
+name = part.split("touches ", 1)[-1].split(" (also")[0].split(" at the grid point")[0]
+at_drop = [ln for ln in text.splitlines() if ln.startswith("release: at drop")]
+check("a drop inside the rover: refused, and at the drop pose why() names the part",
+ code == 1 and part.startswith("forbidden") and at_drop and "REFUSED" in at_drop[0] and name in at_drop[0],
+ name[:90])
+code, text = plan("--speed", "900", "--clearance", "0")
+check("--speed 900 without clearance: refused", code == 1 and "needs --clearance" in text)
+code, text = D.main(["plan", "--table", str(table_path), "--calib", str(TMP / "none.json")], say=lambda *_: None), ""
+check("no calibration: no plan (exit 2)", code == 2)
+
+# ── G. the CAD scene ──
+print("G. the CAD scene")
+try:
+ import mujoco
+except ImportError:
+ mujoco = None
+cad = D.find_cad("auto")
+if mujoco is None:
+ print(" -- skipped: mujoco not installed")
+elif cad is None or not cad.is_file():
+ print(" -- skipped, no CAD scene here (MOSS_ARM_CAD, or ~/moss-*/cad/*/MOSS_R31.xml)")
+else:
+ print(f" model {cad.name}")
+ import gc
+ import moss_arm_keepout as K
+ K._lean_malloc() # a dropped model's memory goes back to the system
+ m = mujoco.MjModel.from_xml_path(str(cad))
+ d = mujoco.MjData(m)
+ tool = m.body(D.TOOL_BODY).id
+ err = {}
+ for n, p in DOC["poses"].items():
+ for j, r in zip(JOINTS, p["rad"]):
+ d.qpos[m.jnt_qposadr[m.joint(j).id]] = r
+ mujoco.mj_kinematics(m, d)
+ tcp = d.xpos[tool] + d.xmat[tool].reshape(3, 3) @ np.array(D.TCP_IN_TOOL_M)
+ err[n] = float(np.linalg.norm(tcp - np.array(p["tcp_m"])))
+ del m, d
+ gc.collect()
+ K._trim()
+ check("every delivered pose's tool point lands on its tcp_m within 1 mm",
+ max(err.values()) < 0.001, ", ".join(f"{n} {1000 * e:.2f}" for n, e in err.items()) + " mm")
+ c = D.Checker(GUARD, CAL, TOL, 1)
+ scene, _ = quiet(D.scene_check, c, moves, cad)
+ total = sum(r["arm"] + r["shoulder"] + r["can"] + r["errors"] for r in scene.contacts)
+ n_poses = sum(r["samples"] for r in scene.contacts)
+ check("plan --mujoco: 0 contacts on the whole sequence (the arm, the shoulder, the can while held)",
+ total == 0 and n_poses > 500, f"{n_poses} poses")
+ rest = {n: scene.poses[n] for n in DOC["rest"]}
+ check("the poses to wait in (drive, clear): the tool point within 12 cm of the pan axis",
+ all(p["axis_m"] <= 0.12 for p in rest.values()), ", ".join(f"{n} {100 * p['axis_m']:.1f} cm" for n, p in rest.items()))
+ check("and the shoulder's static load 0.2 N m or less",
+ all(abs(p["shoulder_nm"]) <= 0.2 for p in rest.values()), ", ".join(f"{n} {p['shoulder_nm']:+.2f}" for n, p in rest.items()))
+ pk = scene.poses["pick"]
+ check("the pick (reaching to the floor): more than 15 cm out, more than 0.5 N m",
+ pk["axis_m"] > 0.15 and pk["shoulder_nm"] > 0.5, f"{100 * pk['axis_m']:.1f} cm, {pk['shoulder_nm']:+.2f} N m")
+ rss = resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024
+ check("one process, under 4 GB", rss < 4096, f"{rss:.0f} MB at the peak")
+
+print()
+print(f"{OK} OK, {KO} KO")
+print("TEST PASSED" if KO == 0 else "TEST FAILED")
+_TMP.cleanup()
+sys.exit(1 if KO else 0)
diff --git a/config/arm_drop_poses.json b/config/arm_drop_poses.json
new file mode 100644
index 0000000..1c7f13f
--- /dev/null
+++ b/config/arm_drop_poses.json
@@ -0,0 +1,172 @@
+{
+ "format": "moss-arm-drop/1",
+ "about": "A pick-and-drop sequence: from the drive pose to travel, down to the pick, the gripper closing, up through lift to clear (the arm folded high over the base: the pose to wait in), the base turned to the drop pose over the bin, the gripper opening, back to clear, then to the drive pose. docs/arm.md, section Drop.",
+ "joints": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"],
+ "units": {
+ "rad": "model radians (the keep-out table's model)",
+ "ticks": "servo positions as reported (homing offset applied)",
+ "tcp_m": "metres, the model's world frame: the tool point 35 mm along the approach from the gripper's tool frame",
+ "duration_s": "seconds",
+ "gap_mm": "the gripper's jaw gap in millimetres"
+ },
+ "ticks_rule": "ticks = the table's joint_map on rad (today zero 2048 and sign +1 on every joint: round(2048 + rad * 4096 / (2 pi))); ticks are written here for reading, the tool recomputes them",
+ "interpolation": "each move is a straight line in joint space, timed by the quintic smootherstep u -> 6u^5 - 15u^4 + 10u^3 over its duration; angles are never wrapped (the pan turns the way the numbers say); the jaw gap follows the cubic smoothstep 3u^2 - 2u^3 over gap_duration_s (default the move's)",
+ "gripper": {
+ "open": {"gap_mm": 90, "ticks": 3674},
+ "grasp": {"gap_mm": 53, "ticks": 2780},
+ "force": 400,
+ "force_registers": [44, 48],
+ "rule": "ticks for a gap: the straight line through open and grasp (24.2 ticks per mm); the grasp value is the goal that closed on an object with the force limited, not a measured 53 mm"
+ },
+ "rest": ["drive", "clear"],
+ "poses": {
+ "travel": {
+ "rad": [1.357552549, -0.809755548, 0.383285798, 1.551888428, -0.685999941],
+ "ticks": [2933, 1520, 2298, 3060, 1601],
+ "tcp_m": [0.26, -0.01, 0.245],
+ "shift_deg": {"elbow_flex": -3, "wrist_flex": -6},
+ "shift_why": "the pose as delivered is refused at clearance 1: forbidden: the arm touches SO101 base link 1 at the grid point shoulder_pan +1.270, shoulder_lift -0.640, elbow_flex +0.622, wrist_flex +1.833, wrist_roll -1.232 rad"
+ },
+ "pick": {
+ "rad": [1.340075828, 0.616745644, -0.066675235, 1.020729568, -1.585330564],
+ "ticks": [2922, 2450, 2005, 2713, 1015],
+ "tcp_m": [0.345, -0.01, 0.082],
+ "shift_deg": {"elbow_flex": -6},
+ "shift_why": "the pose as delivered is refused at clearance 1: within 1 grid step(s) of a forbidden grid point (clearance 1): the arm touches Hull R23 - open battery bay smooth underside and driver access at the grid point shoulder_pan +0.988, shoulder_lift +0.640, elbow_flex +0.124, wrist_flex +1.494, wrist_roll -2.082 rad"
+ },
+ "lift": {
+ "rad": [1.345484884, -0.64214476, 0.107309414, 1.319621434, -0.6859997],
+ "ticks": [2925, 1629, 2118, 2908, 1601],
+ "tcp_m": [0.31, -0.01, 0.295]
+ },
+ "clear": {
+ "rad": [1.411992262, -1.233952819, -0.344509226, 1.651086912, -1.585357879],
+ "ticks": [2968, 1244, 1823, 3124, 1015],
+ "tcp_m": [0.18, -0.01, 0.405],
+ "shift_deg": {"shoulder_lift": 7},
+ "shift_why": "the pose as delivered is refused at clearance 1: within 1 grid step(s) of a forbidden grid point (clearance 1): the arm touches R8H reference - bin corrected printed at the grid point shoulder_pan +0.988, shoulder_lift -1.664, elbow_flex -0.622, wrist_flex +1.156, wrist_roll -2.932 rad"
+ },
+ "drop": {
+ "rad": [-1.847528266, -0.78564082, -0.181778048, 1.287015828, -0.800001218],
+ "ticks": [844, 1536, 1929, 2887, 1526],
+ "tcp_m": [-0.071, -0.005, 0.385],
+ "shift_deg": {"shoulder_lift": -9, "wrist_flex": -10},
+ "shift_why": "the pose as delivered is refused at clearance 1: forbidden: the arm touches R8H reference - bin corrected printed at the grid point shoulder_pan -2.117, shoulder_lift -0.640, elbow_flex -0.124, wrist_flex +1.494, wrist_roll -1.232 rad"
+ }
+ },
+ "sequence": [
+ {
+ "phase": "leave_drive",
+ "from": "drive",
+ "to": "travel",
+ "duration_s": 10.3,
+ "gap_mm": [90, 90],
+ "added": true,
+ "via": [
+ {
+ "rad": [1.345301151, -0.981747704, 0.335941793, 1.122873937, -0.437184525],
+ "ticks": [2925, 1408, 2267, 2780, 1763],
+ "at": 0.9,
+ "shift_deg": {"shoulder_lift": -4, "elbow_flex": -8},
+ "why": "the straight move between its poses is refused at clearance 1"
+ }
+ ]
+ },
+ {
+ "phase": "approach",
+ "from": "travel",
+ "to": "pick",
+ "duration_s": 3,
+ "gap_mm": [90, 90],
+ "via": [
+ {
+ "rad": [1.346835132, -0.05829127, -0.170271867, 1.170427341, -1.270136092],
+ "ticks": [2926, 2010, 1937, 2811, 1220],
+ "at": 0.65,
+ "shift_deg": {"shoulder_lift": -10, "elbow_flex": -10},
+ "why": "the straight move between its poses is refused at clearance 1"
+ }
+ ]
+ },
+ {"phase": "grasp", "from": "pick", "to": "pick", "duration_s": 1, "gap_mm": [90, 53]},
+ {
+ "phase": "lift",
+ "from": "pick",
+ "to": "lift",
+ "duration_s": 2.6,
+ "gap_mm": [53, 53],
+ "holds_can": true,
+ "via": [
+ {
+ "rad": [1.342233189, 0.302194215, -0.171805848, 1.043106936, -1.359106978],
+ "ticks": [2923, 2245, 1936, 2728, 1162],
+ "at": 0.25,
+ "shift_deg": {"elbow_flex": -4, "wrist_flex": -3},
+ "why": "the straight move between its poses is refused at clearance 1"
+ }
+ ]
+ },
+ {"phase": "clear", "from": "lift", "to": "clear", "duration_s": 2.2, "gap_mm": [53, 53], "holds_can": true},
+ {
+ "phase": "transfer",
+ "from": "clear",
+ "to": "drop",
+ "duration_s": 3.2,
+ "gap_mm": [53, 53],
+ "holds_can": true,
+ "via": [
+ {
+ "rad": [-0.730174855, -0.99248557, -0.231631099, 1.098330244, -1.035437032],
+ "ticks": [1572, 1401, 1897, 2764, 1373],
+ "at": 0.7,
+ "shift_deg": {"shoulder_pan": 8, "wrist_flex": -10},
+ "why": "the straight move between its poses is refused at clearance 1"
+ }
+ ]
+ },
+ {
+ "phase": "release",
+ "from": "drop",
+ "to": "drop",
+ "duration_s": 1,
+ "gap_mm": [53, 90],
+ "holds_can": true,
+ "gap_duration_s": 0.7
+ },
+ {
+ "phase": "back_to_clear",
+ "from": "drop",
+ "to": "clear",
+ "duration_s": 2.2,
+ "gap_mm": [90, 90],
+ "via": [
+ {
+ "rad": [-0.730174855, -0.99248557, -0.231631099, 1.098330244, -1.035437032],
+ "ticks": [1572, 1401, 1897, 2764, 1373],
+ "at": 0.3,
+ "shift_deg": {"shoulder_pan": 8, "wrist_flex": -10},
+ "why": "the straight move between its poses is refused at clearance 1"
+ }
+ ]
+ },
+ {"phase": "hold", "from": "clear", "to": "clear", "duration_s": 0.8, "gap_mm": [90, 90]},
+ {
+ "phase": "to_drive",
+ "from": "clear",
+ "to": "drive",
+ "duration_s": 10.6,
+ "gap_mm": [90, 90],
+ "added": true,
+ "via": [
+ {
+ "rad": [1.377514748, -1.096796263, 0.167203906, 0.958737992, -0.908116626],
+ "ticks": [2946, 1333, 2157, 2673, 1456],
+ "at": 0.2,
+ "shift_deg": {"shoulder_lift": 9, "elbow_flex": 5},
+ "why": "the straight move between its poses is refused at clearance 1"
+ }
+ ]
+ }
+ ],
+ "proposal": "shift_deg on a pose and via on a move are the smallest changes found by tools/moss_arm_drop.py plan --suggest that pass every check at clearance 1 with a 25-tick tolerance; plan --as-delivered leaves them out"
+}
diff --git a/docs/arm.md b/docs/arm.md
index 1bb5926..52a1456 100644
--- a/docs/arm.md
+++ b/docs/arm.md
@@ -432,3 +432,28 @@ pose or a wrong joint).
4. Commit `config/arm_keepout.json`. Its `model.sha256` and
`model.geometry_sha256` say which scene it came from (the second one
changes with any mesh); its `joint_map` is the one it replaced.
+
+## Drop
+
+`config/arm_drop_poses.json` holds a pick-and-drop sequence (format
+`moss-arm-drop/1`): drive, travel, pick (the gripper closes), lift, clear
+(the arm folded high over the base: the pose to wait in), drop over the bin
+(the gripper opens), clear, drive. Each pose is five model radians (`rad`),
+its ticks through the table's `joint_map` (`ticks`, recomputed by the tool)
+and its tool point in the model's world frame (`tcp_m`). Each move of
+`sequence` names its poses, its duration, the jaw gap at both ends
+(`gap_mm`, turned into gripper ticks by the line through `open` and
+`grasp` of `gripper`) and whether the can is held. A move is a straight line
+in joint space timed by a quintic smootherstep; the pan is never wrapped.
+`shift_deg` on a pose and `via` on a move are the smallest changes that pass
+the guard at clearance 1; `--as-delivered` leaves them out.
+
+```sh
+python tools/moss_arm_drop.py plan [--as-delivered] [--clearance 1] [--suggest] [--mujoco auto]
+python benches/bench_arm_drop_cold.py
+```
+
+`plan` moves nothing: it checks every sample of every move (a quarter of a
+grid step apart at most) with the guard, `segment_allowed` and the sweep's
+box rule, prints one line per move with the reason of a refusal, and with
+`--mujoco` counts the contacts of the whole sequence in the CAD scene.
diff --git a/tools/moss_arm_drop.py b/tools/moss_arm_drop.py
new file mode 100644
index 0000000..da9a031
--- /dev/null
+++ b/tools/moss_arm_drop.py
@@ -0,0 +1,685 @@
+#!/usr/bin/env python3
+"""The arm's pick-and-drop sequence, checked against the keep-out guard before it moves.
+
+The sequence (config/arm_drop_poses.json) is a list of joint-space moves
+between named poses: from the drive pose to a travel pose, down to the pick,
+the gripper closing, up through lift to clear (the arm folded high over the
+base), the base turned to the drop pose over the bin, the gripper opening,
+back to clear, and from clear to the drive pose. Each move is a straight line
+in joint space, timed by a quintic smootherstep (zero speed and acceleration
+at both ends); the pan is never wrapped: the route turns the way the angles
+say, never the short way round.
+
+ tools/moss_arm_drop.py plan # the guard on every move, one line per move
+ tools/moss_arm_drop.py plan --suggest # also the smallest change that would pass, for each refusal
+ tools/moss_arm_drop.py plan --as-delivered # the poses as delivered, without the shifts and via poses
+ tools/moss_arm_drop.py plan --mujoco auto # also every sample posed in the CAD scene: contacts, tool point, shoulder load
+
+Options
+ --poses the sequence (config/arm_drop_poses.json)
+ --table the keep-out table (config/arm_keepout.json)
+ --calib the arm's calibration (config/arm_calibration.json): the drive pose,
+ the servo IDs and their ranges
+ --clearance grid steps kept clear of any forbidden grid point (1): the table's
+ forbidden points grown by that many steps along each joint
+ --tolerance ticks: the box rule's margin around every sample (25)
+ --speed ticks per second: the fastest any joint may go (400); a move the
+ sequence times faster is stretched; above 400 needs --clearance 1
+ --as-delivered the poses without their shift_deg and the moves without their via poses
+ --suggest for each refused pose, the smallest shift (whole degrees, one or
+ two joints) that passes; for each refused move between two poses
+ that pass, the via pose (a point of the move, shifted the same way)
+ that splits it into two moves that pass
+ --mujoco a MOSS_R31.xml scene (auto: MOSS_ARM_CAD, else ~/moss-*/cad/*/MOSS_R31.xml)
+
+What plan checks
+ The guard's table (moss_dimos/arm_guard.py) with its forbidden grid points
+ grown by --clearance. Every move is cut into samples, servo ticks rounded as
+ they would be sent, no joint moving more than a quarter of a grid step from
+ one sample to the next. Each sample: inside the calibrated range of every
+ joint, and allowed by the guard's rule. Each pair of samples: the straight
+ move between them allowed (segment_allowed), and the box rule of
+ tools/moss_arm_sweep.py: every grid point of the box the pair spans, each
+ end widened by --tolerance (the drive pose's end by nothing: it is the pose
+ itself), is free. The first sample that fails names the reason and the
+ rover part (the nearest forbidden grid point of the table itself when the
+ refusal comes from the clearance). Exit status 0 when every move passes,
+ 1 when one is refused, 2 when the inputs do not load.
+
+ The timing: each move keeps the sequence's duration unless a joint would go
+ faster than --speed at the quintic's peak (1.875 times the mean speed):
+ then it is stretched to that speed. The table knows poses, not overshoot:
+ above 400 ticks per second the plan needs --clearance 1, as the sweep does.
+
+ --mujoco poses every sample again (finer: one degree per joint at most) in
+ the CAD scene, as tools/moss_arm_keepout.py builds it (the rover's parts,
+ the arm with its fingers swept over every opening, the shoulder on its own,
+ and the can of the table while the gripper holds it), and counts the
+ contacts closer than the table's margin: zero expected. It also gives, for
+ each pose, the tool point (in the model's world frame), its horizontal
+ distance from the pan axis, and the shoulder's static load: the gravity
+ torque of the links below the shoulder joint, from the model's masses
+ (an order of magnitude: the model's masses are not weighed parts). It
+ needs mujoco and runs at nice 10, one process.
+"""
+from __future__ import annotations
+
+import argparse
+import copy
+import glob
+import itertools
+import json
+import math
+import os
+import sys
+from dataclasses import dataclass, field
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parent.parent
+sys.path.insert(0, str(ROOT))
+sys.path.insert(0, str(ROOT / "tools"))
+
+import moss_arm_sweep as W # noqa: E402 (the planner's clearance and box rule, the calibration loader)
+
+POSES = ROOT / "config" / "arm_drop_poses.json"
+FORMAT = "moss-arm-drop/1"
+TICKS_PER_REV = 4096
+SAMPLES_PER_STEP = 4 # a move is cut so that no joint moves more than 1/4 of a grid step between samples
+QUINTIC_PEAK = 1.875 # the quintic smootherstep's peak speed over its mean speed
+MJ_STEP_DEG = 1.0 # --mujoco: at most this many degrees per joint between two posed samples
+SHIFT_MAX_DEG = 30 # --suggest: the largest shift tried, whole degrees
+VIA_FRACTIONS = [0.5, 0.45, 0.55, 0.4, 0.6, 0.35, 0.65, 0.3, 0.7, 0.25, 0.75, 0.2, 0.8, 0.15, 0.85, 0.1, 0.9]
+TCP_IN_TOOL_M = (0.0, 0.0, -0.035) # the tool point, in the gripper's tool frame (35 mm along the approach)
+TOOL_BODY = "nc90_tool"
+LOAD_JOINT = "shoulder_lift"
+PAN_JOINT = "shoulder_pan"
+GRAVITY = 9.81
+
+
+class Refused(Exception):
+ """The inputs do not make a plan."""
+
+
+def quintic(u: float) -> float:
+ u = min(max(float(u), 0.0), 1.0)
+ return u * u * u * (u * (u * 6.0 - 15.0) + 10.0)
+
+
+def cubic(u: float) -> float:
+ u = min(max(float(u), 0.0), 1.0)
+ return u * u * (3.0 - 2.0 * u)
+
+
+# ── the sequence ──
+
+@dataclass
+class Pose:
+ name: str
+ ticks: dict # as used: the shift applied unless --as-delivered
+ delivered: dict # ticks of the pose as delivered
+ shift_deg: dict = field(default_factory=dict)
+ added: bool = False # a pose of ours (drive, a via pose), not of the delivered sequence
+
+
+@dataclass
+class Move:
+ phase: str
+ a: Pose
+ b: Pose
+ duration_s: float # the sequence's own duration for this move
+ gap_mm: tuple # the jaw gap at the start and at the end
+ holds_can: bool # the gripper holds the can during this move
+ added: bool = False
+
+
+def gap_to_ticks(gripper: dict, gap_mm: float) -> int:
+ """The gripper's goal for a jaw gap: the straight line through its open and grasp points."""
+ o, g = gripper["open"], gripper["grasp"]
+ k = (float(o["ticks"]) - float(g["ticks"])) / (float(o["gap_mm"]) - float(g["gap_mm"]))
+ return int(round(float(g["ticks"]) + k * (float(gap_mm) - float(g["gap_mm"]))))
+
+
+def load_doc(path) -> dict:
+ try:
+ doc = json.loads(Path(path).read_text())
+ except (OSError, ValueError) as e:
+ raise Refused(f"the sequence {path} does not load: {e}") from e
+ if doc.get("format") != FORMAT:
+ raise Refused(f"{path} is not a drop sequence (format {doc.get('format')!r}, want {FORMAT!r})")
+ return doc
+
+
+def build(doc: dict, jm, joints, drive: dict, as_delivered: bool = False) -> list:
+ """The moves, in order, from the parsed sequence (angles in model radians,
+ turned into ticks through jm, the table's JointMap): each via pose splits
+ its move into two, the duration shared in proportion to each piece's
+ largest joint move."""
+ joints = list(joints)
+ if list(doc.get("joints", [])) != joints:
+ raise Refused(f"the sequence's joints {doc.get('joints')} are not the table's {joints}")
+
+ def ticks(rad, shift):
+ if len(rad) != len(joints):
+ raise Refused(f"{len(rad)} angles for {len(joints)} joints")
+ unknown = [j for j in shift if j not in joints]
+ if unknown:
+ raise Refused(f"shift_deg names {unknown}")
+ return {j: jm.rad_to_ticks(j, float(v) + math.radians(float(shift.get(j, 0.0)))) for j, v in zip(joints, rad)}
+
+ poses = {"drive": Pose("drive", dict(drive), dict(drive), added=True)}
+ for name, p in doc["poses"].items():
+ shift = {} if as_delivered else {j: float(v) for j, v in (p.get("shift_deg") or {}).items()}
+ poses[name] = Pose(name, ticks(p["rad"], shift), ticks(p["rad"], {}), shift)
+ moves = []
+ for k, s in enumerate(doc["sequence"]):
+ for key in ("from", "to"):
+ if s[key] not in poses:
+ raise Refused(f"move {k} ({s.get('phase')}): no pose {s[key]!r}")
+ a, b = poses[s["from"]], poses[s["to"]]
+ gap = tuple(float(v) for v in s.get("gap_mm", (None, None)))
+ pts = [a]
+ for i, v in enumerate([] if as_delivered else (s.get("via") or [])):
+ t = ticks(v["rad"], {})
+ pts.append(Pose(f"via {a.name}>{b.name}" + (f" {i + 1}" if i else ""), t, t, added=True))
+ pts.append(b)
+ spans = [max(abs(q.ticks[j] - p.ticks[j]) for j in joints) for p, q in zip(pts, pts[1:])]
+ total = sum(spans)
+ for p, q, d in zip(pts, pts[1:], spans):
+ share = d / total if total else 1.0 / len(spans)
+ moves.append(Move(s["phase"], p, q, float(s["duration_s"]) * share, gap,
+ bool(s.get("holds_can", False)), bool(s.get("added", False))))
+ return moves
+
+
+def samples(a: dict, b: dict, joints, step: dict, per_step: int = SAMPLES_PER_STEP) -> list:
+ """The move a -> b cut so that no joint moves more than step[j] / per_step
+ ticks between two samples, rounded to ticks (half up, in exact integers:
+ samples(b, a) is samples(a, b) reversed); a and b included."""
+ most = {j: max(1, int(step[j] // per_step)) for j in joints} # ticks: the rounding never steps past it
+ n = max(1, max(int(math.ceil(abs(b[j] - a[j]) / most[j])) for j in joints))
+ return [{j: (2 * (int(a[j]) * n + (int(b[j]) - int(a[j])) * k) + n) // (2 * n) for j in joints} for k in range(n + 1)]
+
+
+def peak_speed(m: Move, joints) -> float:
+ """Ticks per second at the quintic's peak, on the joint that moves most."""
+ d = max(abs(m.b.ticks[j] - m.a.ticks[j]) for j in joints)
+ return QUINTIC_PEAK * d / m.duration_s if m.duration_s > 0 else (math.inf if d else 0.0)
+
+
+def stretched(m: Move, joints, speed: float) -> float:
+ d = max(abs(m.b.ticks[j] - m.a.ticks[j]) for j in joints)
+ return max(m.duration_s, QUINTIC_PEAK * d / speed) if d else m.duration_s
+
+
+# ── the checks ──
+
+class Checker:
+ """The guard's rule, segment_allowed and the sweep's box rule, on the
+ table's forbidden grid points grown by `clearance` grid steps."""
+
+ def __init__(self, guard, calibration: dict, tolerance: int = 25, clearance: int = 1):
+ self.guard, self.tol, self.clearance = guard, int(tolerance), int(clearance)
+ model = W.SpeedModel(W.SLOW_SPEED, 8, self.tol)
+ grown = W.Planner(guard, calibration, model, self.tol, self.clearance) # the sweep's clearance
+ self.used = copy.copy(guard)
+ self.used.occ = grown.occ_used
+ self.used._targets = None
+ self.p = W.Planner(self.used, calibration, model, self.tol, 0) # its box rule on that table
+ self.joints = list(guard.joints)
+ self.drive = dict(self.p.drive)
+ self.ranges = dict(self.p.ranges)
+ self.step = {j: s for j, s in zip(self.joints, self.p.step_ticks)}
+
+ def rad(self, t: dict) -> dict:
+ return self.p.rad(t)
+
+ def why(self, t: dict) -> str:
+ """Why a pose is refused: the table's own why() where the table refuses
+ it, else the forbidden grid point of the table the clearance grew from."""
+ q = self.rad(t)
+ if not self.guard.allowed(q):
+ return self.guard.why(q)
+ f = self.guard._fracs(q)
+ if self.used._outside(f) is not None:
+ return self.used.why(q)
+ return self._grown_why(self.guard._axes(f), f)
+
+ def _grown_why(self, axes, f) -> str:
+ c = self.clearance
+ idx = [np.arange(max(min(a) - c, 0), min(max(a) + c, n - 1) + 1) for a, n in zip(axes, self.guard.shape)]
+ sub = self.guard.occ[np.ix_(*idx)]
+ pts = np.argwhere(sub) + np.array([i[0] for i in idx])
+ if not len(pts):
+ return "refused (no forbidden grid point of the table nearby: check the clearance)"
+ d = np.abs(pts - f).max(axis=1)
+ p = tuple(int(x) for x in pts[int(np.argmin(d))])
+ q = {j: float(g[p[k]]) for k, (j, g) in enumerate(zip(self.joints, self.guard.grids))}
+ return (f"within {c} grid step(s) of a forbidden grid point (clearance {c}): "
+ + self.guard.why(q).replace("forbidden: ", ""))
+
+ def in_range(self, t: dict) -> str | None:
+ out = [f"{j} {t[j]} outside {lo}..{hi}" for j, (lo, hi) in self.ranges.items() if not lo <= t[j] <= hi]
+ return "; ".join(out) or None
+
+ def robust(self, t: dict) -> bool:
+ """The pose and its box widened by the tolerance pass (a pose to stop at)."""
+ return self.in_range(t) is None and self.used.allowed(self.rad(t)) and self.p.box_free(t, t)
+
+ def pair(self, a: dict, b: dict) -> str | None:
+ """None when the step a -> b passes every check, else why not."""
+ for t in (a, b):
+ r = self.in_range(t)
+ if r:
+ return f"outside the calibrated range: {r}"
+ if not self.used.allowed(self.rad(t)):
+ return f"the pose is refused: {self.why(t)}"
+ if not self.used.segment_allowed(self.rad(a), self.rad(b)):
+ return f"segment_allowed refuses the step: {self.why(b)}"
+ if not self.p.box_free(a, b):
+ return f"the box rule refuses the step (a forbidden grid point within {self.tol} ticks): {self._box_why(a, b)}"
+ return None
+
+ def _box_why(self, a: dict, b: dict) -> str:
+ qa, qb = self.rad(a), self.rad(b)
+ pad = self.tol * 2.0 * math.pi / TICKS_PER_REV
+ pa = 0.0 if a == self.drive else pad
+ pb = 0.0 if b == self.drive else pad
+ idx = []
+ for j, g in zip(self.joints, self.guard.grids):
+ lo, hi = min(qa[j] - pa, qb[j] - pb), max(qa[j] + pa, qb[j] + pb)
+ tl, th = W._touched(W._frac(g, lo), len(g)), W._touched(W._frac(g, hi), len(g))
+ if tl is None or th is None:
+ return f"{j} leaves the table"
+ idx.append(np.arange(tl[0], th[1] + 1))
+ sub = self.used.occ[np.ix_(*idx)]
+ pts = np.argwhere(sub) + np.array([i[0] for i in idx])
+ if not len(pts):
+ return "nothing found"
+ f = np.array([W._frac(g, (qa[j] + qb[j]) / 2.0) for j, g in zip(self.joints, self.guard.grids)])
+ p = tuple(int(x) for x in pts[int(np.argmin(np.abs(pts - f).max(axis=1)))])
+ q = {j: float(g[p[k]]) for k, (j, g) in enumerate(zip(self.joints, self.guard.grids))}
+ if self.guard.occ[p]:
+ return self.guard.why(q).replace("forbidden: ", "")
+ return self._grown_why([(i,) for i in p], np.array(p, dtype=float))
+
+ def move(self, a: dict, b: dict) -> tuple[int, str | None, float | None]:
+ """(samples, why the first failing step fails or None, where along the move 0..1)."""
+ pts = samples(a, b, self.joints, self.step)
+ n = len(pts) - 1
+ for k in range(n):
+ why = self.pair(pts[k], pts[k + 1])
+ if why:
+ return n + 1, why, (k + 1) / n if n else 1.0
+ if n == 0:
+ why = self.pair(pts[0], pts[0])
+ if why:
+ return 1, why, 0.0
+ return n + 1, None, None
+
+
+# ── the smallest change ──
+
+def _shifts(max_deg: int):
+ """(joint indices, degrees) for one or two joints, by the largest shift
+ first, then the sum, whole degrees up to max_deg."""
+ out = []
+ vals = list(range(-max_deg, max_deg + 1))
+ for k in (1, 2):
+ for js in itertools.combinations(range(5), k):
+ for combo in itertools.product(vals, repeat=k):
+ if 0 in combo:
+ continue
+ out.append((max(map(abs, combo)), sum(map(abs, combo)), k, js, combo))
+ out.sort(key=lambda x: x[:3])
+ return out
+
+
+def _shifted(c: Checker, t: dict, js, combo) -> dict:
+ """t with joints js turned by combo degrees (through the table's JointMap)."""
+ out = dict(t)
+ jm = c.guard.joint_map
+ for i, d in zip(js, combo):
+ j = c.joints[i]
+ out[j] = jm.rad_to_ticks(j, jm.ticks_to_rad(j, t[j]) + math.radians(d))
+ return out
+
+
+def suggest_pose(c: Checker, t: dict, max_deg: int = SHIFT_MAX_DEG):
+ """The smallest shift ({joint: degrees}) after which the pose passes with
+ its tolerance box, or None."""
+ for m, _, _, js, combo in _shifts(max_deg):
+ if c.robust(_shifted(c, t, js, combo)):
+ return {c.joints[i]: d for i, d in zip(js, combo)}
+ return None
+
+
+def suggest_via(c: Checker, a: dict, b: dict, max_deg: int = SHIFT_MAX_DEG):
+ """A via pose (ticks) on the move a -> b, shifted by the smallest whole
+ degrees on one or two joints, that splits it into two moves that pass;
+ (via ticks, fraction, shift) or None."""
+ shifts = [(0, 0, 0, (), ())] + _shifts(max_deg)
+ for m in range(0, max_deg + 1):
+ level = [s for s in shifts if s[0] == m]
+ for f in VIA_FRACTIONS:
+ base = {j: int(round(a[j] + (b[j] - a[j]) * f)) for j in c.joints}
+ for _, _, _, js, combo in level:
+ v = _shifted(c, base, js, combo)
+ if not c.robust(v):
+ continue
+ if not (c.used.segment_allowed(c.rad(a), c.rad(v)) and c.used.segment_allowed(c.rad(v), c.rad(b))):
+ continue
+ if c.move(a, v)[1] is None and c.move(v, b)[1] is None:
+ return v, f, {c.joints[i]: d for i, d in zip(js, combo)}
+ return None
+
+
+# ── the plan ──
+
+@dataclass
+class Verdict:
+ move: Move
+ samples: int
+ why: str | None
+ at: float | None
+ duration_s: float
+ peak: float
+
+
+def check_moves(c: Checker, moves: list, speed: float) -> list:
+ out = []
+ for m in moves:
+ n, why, at = c.move(m.a.ticks, m.b.ticks)
+ out.append(Verdict(m, n, why, at, stretched(m, c.joints, speed), peak_speed(m, c.joints)))
+ return out
+
+
+def _deg(t: dict, c: Checker) -> str:
+ return " ".join(f"{math.degrees(v):+7.1f}" for v in c.rad(t).values())
+
+
+def print_plan(c: Checker, verdicts: list, doc: dict, speed: float, say=print) -> None:
+ say(f"table: {c.guard.summary()}")
+ say(f"checks: clearance {c.clearance} grid step(s) (grid steps "
+ + ", ".join(f"{j} {s:.0f}" for j, s in c.step.items()) + " ticks), "
+ f"box rule tolerance {c.tol} ticks, samples at most a quarter of a grid step apart")
+ say("poses (ticks; model degrees " + " ".join(j[:8] for j in c.joints) + "):")
+ seen = {}
+ for v in verdicts:
+ for p in (v.move.a, v.move.b):
+ seen.setdefault(p.name, p)
+ for p in seen.values():
+ sh = (" shifted " + ", ".join(f"{j} {d:+g} deg" for j, d in p.shift_deg.items())
+ + f" (delivered {[p.delivered[j] for j in c.joints]})") if p.shift_deg else ""
+ say(f" {p.name:16s} {str([p.ticks[j] for j in c.joints]):29s} {_deg(p.ticks, c)}{sh}")
+ g = doc.get("gripper") or {}
+ if g:
+ say(f"gripper: open {g['open']['gap_mm']:g} mm = {g['open']['ticks']} ticks, grasp {g['grasp']['gap_mm']:g} mm "
+ f"= {g['grasp']['ticks']} ticks (force {g.get('force')} at registers {g.get('force_registers')})")
+ say(f"{'move':40s} {'samples':>7s} {'seq s':>6s} {'peak t/s':>8s} {'run s':>6s} verdict")
+ t_seq = t_run = 0.0
+ for v in verdicts:
+ m = v.move
+ name = f"{m.phase}: {m.a.name} -> {m.b.name}" if m.a is not m.b and m.a.name != m.b.name else f"{m.phase}: at {m.a.name}"
+ if m.gap_mm[0] != m.gap_mm[1] and g:
+ name += f" (gap {m.gap_mm[0]:g} -> {m.gap_mm[1]:g} mm)"
+ verdict = "OK" if v.why is None else f"REFUSED at {v.at:.2f}: {v.why}"
+ say(f"{name[:40]:40s} {v.samples:7d} {m.duration_s:6.2f} {v.peak:8.0f} {v.duration_s:6.2f} {verdict}")
+ t_seq += m.duration_s
+ t_run += v.duration_s
+ bad = [v for v in verdicts if v.why]
+ say(f"sequence: {len(verdicts)} moves, {t_seq:.1f} s as timed, {t_run:.1f} s with no joint over {speed:g} ticks/s; "
+ + ("every move passes" if not bad else f"{len(bad)} move(s) refused"))
+
+
+def print_suggestions(c: Checker, verdicts: list, say=print) -> dict:
+ """For each refused pose its smallest shift, then for each refused move
+ between poses that pass (shifts applied) a via pose."""
+ out = {"shift_deg": {}, "via": {}}
+ names = {}
+ for v in verdicts:
+ for p in (v.move.a, v.move.b):
+ names.setdefault(p.name, p)
+ fixed = {}
+ for name, p in names.items():
+ if c.robust(p.ticks):
+ fixed[name] = p.ticks
+ continue
+ s = suggest_pose(c, p.ticks)
+ if s is None:
+ say(f"suggest: pose {name}: no shift of one or two joints up to {SHIFT_MAX_DEG} deg passes ({c.why(p.ticks)})")
+ continue
+ fixed[name] = _shifted(c, p.ticks, [c.joints.index(j) for j in s], list(s.values()))
+ total = {j: p.shift_deg.get(j, 0.0) + d for j, d in s.items()}
+ out["shift_deg"][name] = total
+ say(f"suggest: pose {name}: shift " + ", ".join(f"{j} {d:+d} deg" for j, d in s.items())
+ + f" -> {[fixed[name][j] for j in c.joints]} ({c.why(p.ticks)})")
+ for v in verdicts:
+ a, b = v.move.a.name, v.move.b.name
+ if a not in fixed or b not in fixed or a == b:
+ continue
+ ta, tb = fixed[a], fixed[b]
+ if c.move(ta, tb)[1] is None:
+ continue
+ r = suggest_via(c, ta, tb)
+ if r is None:
+ say(f"suggest: move {a} -> {b}: no via pose found (shifts up to {SHIFT_MAX_DEG} deg)")
+ continue
+ via, f, s = r
+ out["via"][f"{a}->{b}"] = {"ticks": [via[j] for j in c.joints], "at": f, "shift_deg": s}
+ say(f"suggest: move {a} -> {b}: a via pose at {f:.2f} of the move"
+ + (", shifted " + ", ".join(f"{j} {d:+d} deg" for j, d in s.items()) if s else "")
+ + f": {[via[j] for j in c.joints]} ({_deg(via, c)} deg)")
+ return out
+
+
+# ── the CAD scene ──
+
+def find_cad(arg: str) -> Path | None:
+ if arg and arg != "auto":
+ return Path(arg)
+ env = os.environ.get("MOSS_ARM_CAD", "").strip()
+ if env:
+ return Path(env)
+ found = sorted(glob.glob(os.path.expanduser("~/moss-*/cad/*/MOSS_R31.xml")))
+ return Path(found[-1]) if found else None
+
+
+@dataclass
+class SceneReport:
+ contacts: list # per move: {"arm": n, "shoulder": n, "can": n, "parts": {name: n}, "samples": n, "errors": n}
+ poses: dict # name: {"tcp_m", "axis_m", "height_m", "shoulder_nm", "lever_m"}
+ margin_m: float
+ model: str
+
+
+def scene_check(c: Checker, moves: list, model_path: Path, say=print) -> SceneReport:
+ """Every move posed in the CAD scene, one degree per joint at most between
+ samples; contacts closer than the table's margin between the rover's parts
+ and the arm (fingers swept), the shoulder (as the table counts it: every
+ contact), and the table's can while the gripper holds it."""
+ import gc
+
+ import mujoco
+ import moss_arm_keepout as K
+
+ if os.nice(0) < 10:
+ os.nice(10 - os.nice(0))
+ K._lean_malloc()
+ layout = K.layout_of(model_path)
+ L = K.LAYOUTS[layout]
+ margin = float(c.guard.meta.get("margin_m") or L["margin"])
+ pay = (c.guard.meta.get("payload") or {})
+ payload = K.parse_payload(pay.get("kind", "none")) if pay else None
+ if payload and pay.get("diameter_m"):
+ payload = dict(payload, diameter_m=float(pay["diameter_m"]), height_m=float(pay["height_m"]))
+ manifest = model_path.parent / "parts_manifest.json"
+ names = K.load_names(manifest) if manifest.is_file() else {}
+ p = K.prepare(model_path, arm_root=L["arm_root"], rover_body=L["rover_body"],
+ rover_parts=tuple(s.strip() for s in L["rover_parts"].split(",") if s.strip()),
+ margin=margin, lidar_xyz=None, joints=c.joints, visual=L["visual"],
+ root_voxel=L["root_voxel"], names=names, payload=payload)
+ model = p.model
+ jid = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, j) for j in c.joints]
+ adr = [int(model.jnt_qposadr[j]) for j in jid]
+ is_arm, is_part = K._masks(model, p.arm_ids, p.part_ids)
+ is_root, _ = K._masks(model, p.root_ids, p.part_ids)
+ pay_ids = list(p.payload_ids.values()) if p.payload_model is not None else []
+ is_pay, _ = K._masks(model, pay_ids, p.part_ids) if pay_ids else (None, None)
+ part_name = {g: p.display[k] for g, k in zip(p.part_ids, p.part_label)}
+ d_arm, d_root = mujoco.MjData(model), mujoco.MjData(p.root_model)
+ d_pay = mujoco.MjData(p.payload_model) if pay_ids else None
+
+ def pose(data, base, t):
+ data.qpos[:] = base
+ for a, j in zip(adr, c.joints):
+ data.qpos[a] = c.guard.joint_map.ticks_to_rad(j, t[j])
+
+ step = {j: MJ_STEP_DEG * TICKS_PER_REV / 360.0 for j in c.joints}
+ report = []
+ for m in moves:
+ r = {"arm": 0, "shoulder": 0, "can": 0, "parts": {}, "samples": 0, "errors": 0}
+ for t in samples(m.a.ticks, m.b.ticks, c.joints, step, 1):
+ r["samples"] += 1
+ pose(d_arm, p.base_qpos, t)
+ hits = {"arm": K._touch(model, d_arm, is_arm, is_part, margin)}
+ pose(d_root, p.base_qpos, t)
+ hits["shoulder"] = K._touch(p.root_model, d_root, is_root, is_part, math.inf)
+ if m.holds_can and d_pay is not None:
+ pose(d_pay, p.payload_qpos, t)
+ hits["can"] = K._touch(p.payload_model, d_pay, is_pay, is_part, margin)
+ for k, h in hits.items():
+ if h is None:
+ r["errors"] += 1
+ continue
+ r[k] += len(h)
+ for _, g, _ in h:
+ r["parts"][part_name.get(g, str(g))] = r["parts"].get(part_name.get(g, str(g)), 0) + 1
+ report.append(r)
+ # the poses: tool point, distance from the pan axis, the shoulder's static load
+ tool = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, TOOL_BODY)
+ pan = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, PAN_JOINT)
+ lift = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, LOAD_JOINT)
+ below = [b for b in range(model.nbody) if _under(model, b, int(model.jnt_bodyid[lift]))]
+ poses = {}
+ for m in moves:
+ for q in (m.a, m.b):
+ if q.name in poses:
+ continue
+ pose(d_arm, p.base_qpos, q.ticks)
+ mujoco.mj_kinematics(model, d_arm)
+ tcp = d_arm.xpos[tool] + d_arm.xmat[tool].reshape(3, 3) @ np.array(TCP_IN_TOOL_M)
+ ax = d_arm.xanchor[pan]
+ g = np.array([0.0, 0.0, -GRAVITY])
+ tau, mass, mom = 0.0, 0.0, np.zeros(3)
+ for b in below:
+ tau += float(d_arm.xaxis[lift] @ np.cross(d_arm.xipos[b] - d_arm.xanchor[lift], model.body_mass[b] * g))
+ mass += float(model.body_mass[b])
+ mom += model.body_mass[b] * d_arm.xipos[b]
+ v = mom / mass - d_arm.xanchor[lift]
+ v -= (v @ d_arm.xaxis[lift]) * d_arm.xaxis[lift]
+ poses[q.name] = {"tcp_m": [round(float(x), 4) for x in tcp],
+ "axis_m": round(float(np.hypot(*(tcp - ax)[:2])), 4),
+ "height_m": round(float(tcp[2] - ax[2]), 4),
+ "shoulder_nm": round(tau, 3), "lever_m": round(float(np.hypot(v[0], v[1])), 4),
+ "mass_kg": round(mass, 3)}
+ del d_arm, d_root, d_pay, model, p
+ gc.collect()
+ K._trim()
+ return SceneReport(report, poses, margin, model_path.name)
+
+
+def _under(model, b: int, top: int) -> bool:
+ while b > 0:
+ if b == top:
+ return True
+ b = int(model.body_parentid[b])
+ return False
+
+
+def print_scene(s: SceneReport, verdicts: list, say=print) -> None:
+ say(f"CAD scene {s.model}: contacts closer than {s.margin_m * 1000:g} mm (the arm with its fingers swept, "
+ "the shoulder, the can while held)")
+ for v, r in zip(verdicts, s.contacts):
+ m = v.move
+ name = f"{m.phase}: {m.a.name} -> {m.b.name}" if m.a.name != m.b.name else f"{m.phase}: at {m.a.name}"
+ parts = ", ".join(f"{k} x{n}" for k, n in sorted(r["parts"].items(), key=lambda kv: -kv[1])[:3])
+ say(f" {name[:40]:40s} {r['samples']:4d} poses: arm {r['arm']}, shoulder {r['shoulder']}, "
+ f"can {r['can'] if m.holds_can else '-'}" + (f", errors {r['errors']}" if r["errors"] else "")
+ + (f" ({parts})" if parts else ""))
+ total = sum(r["arm"] + r["shoulder"] + r["can"] + r["errors"] for r in s.contacts)
+ say(f" total contacts: {total}")
+ say("poses: the tool point in the model's world frame, its distance from the pan axis, its height over the "
+ "pan axis's anchor, the shoulder's static load (links below the shoulder joint, the model's masses)")
+ for name, p in s.poses.items():
+ say(f" {name:16s} tool {p['tcp_m']} m, {p['axis_m'] * 100:5.1f} cm from the axis, {p['height_m'] * 100:+5.1f} cm; "
+ f"shoulder {p['shoulder_nm']:+.2f} N m ({p['mass_kg']:g} kg, centre of mass {p['lever_m'] * 100:.1f} cm out)")
+
+
+# ── the command line ──
+
+def plan_main(a, say=print) -> int:
+ if a.speed <= 0 or a.speed > W.MAX_SPEED:
+ say(f"refused: --speed {a.speed:g}: above 0, at most {W.MAX_SPEED}")
+ return 2
+ if a.clearance < 0:
+ say("refused: --clearance below 0")
+ return 2
+ try:
+ guard, calibration = W.load(a.table, a.calib)
+ doc = load_doc(a.poses)
+ c = Checker(guard, calibration, a.tolerance, a.clearance)
+ moves = build(doc, guard.joint_map, c.joints, c.drive, a.as_delivered)
+ except (W.Refused, Refused, KeyError, ValueError) as e:
+ say(f"refused: {e}")
+ return 2
+ verdicts = check_moves(c, moves, a.speed)
+ print_plan(c, verdicts, doc, a.speed, say)
+ fast = max((v.peak for v in verdicts), default=0.0)
+ code = 1 if any(v.why for v in verdicts) else 0
+ if a.speed > W.SLOW_SPEED and a.clearance < 1:
+ say(f"refused: --speed {a.speed:g} above {W.SLOW_SPEED} needs --clearance 1 or more (overshoot at speed is not in the table)")
+ code = 1
+ if fast > W.SLOW_SPEED:
+ say(f"note: the sequence's own timing peaks at {fast:.0f} ticks/s; the run times above stretch it to {a.speed:g}")
+ if a.suggest and code:
+ print_suggestions(c, verdicts, say)
+ if a.mujoco:
+ path = find_cad(a.mujoco)
+ if path is None or not path.is_file():
+ say(f"refused: --mujoco: no CAD scene ({a.mujoco}; MOSS_ARM_CAD, or ~/moss-*/cad/*/MOSS_R31.xml)")
+ return 2
+ try:
+ import mujoco # noqa: F401
+ except ImportError:
+ say("refused: --mujoco needs mujoco (pip install mujoco)")
+ return 2
+ s = scene_check(c, moves, path, say)
+ print_scene(s, verdicts, say)
+ if any(r["arm"] + r["shoulder"] + r["can"] + r["errors"] for r in s.contacts):
+ code = 1
+ return code
+
+
+def main(argv=None, say=print) -> int:
+ ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
+ sub = ap.add_subparsers(dest="command", required=True)
+ pp = sub.add_parser("plan", help="check the sequence against the guard (nothing moves)")
+ pp.add_argument("--poses", default=str(POSES))
+ pp.add_argument("--table", default=str(W.TABLE))
+ pp.add_argument("--calib", default=str(W.CALIBRATION))
+ pp.add_argument("--clearance", type=int, default=1)
+ pp.add_argument("--tolerance", type=int, default=25)
+ pp.add_argument("--speed", type=float, default=float(W.SLOW_SPEED))
+ pp.add_argument("--as-delivered", action="store_true")
+ pp.add_argument("--suggest", action="store_true")
+ pp.add_argument("--mujoco", default="", help="a MOSS_R31.xml, or auto")
+ a = ap.parse_args(argv)
+ if a.command == "plan":
+ return plan_main(a, say)
+ return 2
+
+
+if __name__ == "__main__":
+ sys.exit(main())