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.pynew file mode 100644index 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.jsonnew file mode 100644index 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.mdindex 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 onechanges 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.pynew file mode 100644index 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())