Add pair-scoped attached-payload collision checks

README.md

@@ -4,9 +4,9 @@ A serial-chain manipulator stack written **from scratch in Rust** — no ROS, no
 
 ![Demo 3 — UR5e pick-and-place around a pillar. IK, RRT-Connect, scalar S-curve time law, PD + velocity feedforward](docs/demo3.gif)
 
-The kinematic chain (offsets, joint axes, limits, end-effector site) is **extracted from the compiled MuJoCo model** — never hand-entered as DH parameters — so the geometry used by the algorithms is guaranteed identical to the one the physics simulates. Forward kinematics, geometric Jacobians, damped-least-squares inverse kinematics, RRT-Connect with shortcutting, and a rest-to-rest 7-phase scalar S-curve are implemented in this repo. Collision checks call `mj_collision` on interpolated joint-space states. The independent `k` + `urdf-rs` stack is used **only inside the test suite** as a cross-check of FK.
+The kinematic chain (offsets, joint axes, limits, end-effector site) is **extracted from the compiled MuJoCo model** — never hand-entered as DH parameters — so the geometry used by the algorithms is guaranteed identical to the one the physics simulates. Forward kinematics, geometric Jacobians, damped-least-squares inverse kinematics, RRT-Connect with shortcutting, and a rest-to-rest 7-phase scalar S-curve are implemented in this repo. Robot collision checks call `mj_collision` on interpolated joint-space states; Demo 3 additionally uses explicit, pair-scoped `mj_geomDistance` queries for an EE-attached cube proxy. The independent `k` + `urdf-rs` stack is used **only inside the test suite** as a cross-check of FK.
 
-Demo 3 (above): IK solves pick and place poses; the joint-space straight line between them hits a pillar on the table; RRT-Connect carries the cube around it in **~4 ms**; the path coordinate is timed with an S-curve and tracked to **0.004 rad**. Polyline corners are not blended, so the joint trajectory is not globally jerk-limited. The cube is a mocap weld (scripted attach, not contact-rich grasping). Demo 2 (pillar dodge, no object) is in [`docs/demo2.gif`](docs/demo2.gif). Demo 1 (IK target sequence) is in [`docs/demo1.gif`](docs/demo1.gif).
+Demo 3 (above): IK solves pick and place poses; the joint-space straight line between them violates both the robot predicate and the attached cube's 5 mm pillar margin. Fixed-seed RRT-Connect finds a 6-waypoint sampled-clear carry in **5.4–7.8 ms** across two repeated headless runs; the path coordinate is timed with an S-curve and tracked to **0.0067 rad** worst error. Polyline corners are not blended, so the joint trajectory is not globally jerk-limited. The cube is a mocap weld (scripted attach, not contact-rich grasping). Demo 2 (pillar dodge, no object) is in [`docs/demo2.gif`](docs/demo2.gif). Demo 1 (IK target sequence) is in [`docs/demo1.gif`](docs/demo1.gif).
 
 ## Numbers (measured, reproducible via `cargo test`)
 
@@ -24,8 +24,9 @@ Demo 3 (above): IK solves pick and place poses; the joint-space straight line be
 | Demo 2 scalar S-curve time law (seed `20260816`, v≤0.55 a≤1.8 j≤8) | **1751 samples, 3.50 s**, peak joint \|qd\| 0.550 rad/s |
 | Demo 2 tracking (PD + vel FF + gravity compensation) | worst 0.0046 rad · final goal 0.0002 rad |
 | Timed-trajectory determinism | identical `q(t)` given identical seed + limits |
-| Demo 3 carry (pick→place around table pillar, seed `20260816`) | **~4 ms**, 4 waypoints after shortcut |
-| Demo 3 tracking (full pick-and-place) | worst 0.0037 rad |
+| Demo 3 attached-cube carry (pick→place, seed `20260816`) | **6 waypoints, 47 sampled states**, 0/47 payload-margin violations |
+| Demo 3 sampled minimum cube/environment distance (5 mm threshold) | **11.849 mm** vs pillar |
+| Demo 3 tracking (full pick-and-place, representative headless run) | worst 0.0067 rad |
 
 The 1 s median planning-time exit criterion is met by two orders of magnitude. The IK stress test samples targets from valid random joint configurations; the 2.2% non-converged slice is reported without assigning an untested cause. The solver honors limits by construction (clamped every iteration, asserted in tests).
 
@@ -35,6 +36,12 @@ The 1 s median planning-time exit criterion is met by two orders of magnitude. T
 
 The generated [full results](docs/robustness_results.md) and [raw CSV](docs/robustness_results.csv) state their pass thresholds and limitations. This is a simulation stress test, not hardware validation or a sim-to-real guarantee.
 
+## Attached-payload carry check (simulation)
+
+Demo 3 now transforms the compiled 50 mm cube geom from end-effector FK at every carry query and calls MuJoCo's explicit pair-distance API only for cube↔floor, cube↔table, and cube↔pillar. A state is rejected below a declared 5 mm payload margin, while the original robot rule remains a zero-penetration check. Both predicates independently block the 23-sample straight edge. The fixed-seed plan succeeds with 6 shortcut waypoints and 47 densified states; a fresh checker finds zero robot collisions, zero payload-margin violations, and an 11.849 mm minimum sampled cube distance.
+
+The [pre-result protocol](docs/attached_payload_protocol.md) and [result/limitations report](docs/attached_payload_results.md) preserve the exact pair scope, thresholds, negative outcomes, transform regression, and sampled-only claim boundary. This does not model object slip, grasp uncertainty, fingers, continuous swept volume, or payload-versus-robot self-contact.
+
 ## Multi-scene, multi-query extension (simulation)
 
 `multi_query_bench` adds nine fixed scene-query fixtures (three per shipped MJCF scene, six unique joint-pair definitions) while retaining the original declared numeric tracking thresholds. Selected joint pairs are deliberately repeated across scenes to isolate geometry effects. Across five fixed planner seeds per fixture, all **30/30 direct-free** and **15/15 obstructed** trials succeeded. Executed states are checked after every settling, path, and hold step with a threshold of exactly 0.0 m; a case passes only with zero sampled robot contacts whose signed distance is negative. This is a sampled penetration gate, not a positive-clearance certificate.
@@ -92,12 +99,13 @@ Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PAT
 
 - **Chain extraction, not re-modeling.** `Chain::from_mujoco` walks `body_parentid` from the tip body to the world, collecting static transforms, hinge axes, anchors, and limits from the *compiled* model, plus the EE site as the tool frame. One source of truth for geometry.
 - **DLS IK with adaptive damping.** Each step solves `Δq = Jᵀ(JJᵀ + λ²I)⁻¹e` with a diagonal nullspace bias toward a rest pose; λ scales down with the error so the endgame converges Newton-like while near-singular regions stay damped. Joint limits are clamped every iteration; seeded random restarts (TRAC-IK style) recover from bad basins.
-- **RRT-Connect, from scratch.** Two trees grow toward each other (Kuffner & LaValle 2000). `EXTEND` takes one joint-space step; `CONNECT` greedily repeats it. Edges are collision-checked discretely by interpolating at `resolution` (0.05 rad L2 by default) and calling MuJoCo `mj_collision` on each state; this is sampled collision checking, not continuous certification. Greedy then random shortcutting removes redundant waypoints; the path is densified to the same resolution for execution. Sampling uses the in-repo SplitMix64 RNG — same seed, same path.
+- **RRT-Connect, from scratch.** Two trees grow toward each other (Kuffner & LaValle 2000). `EXTEND` takes one joint-space step; `CONNECT` greedily repeats it. Edges are collision-checked discretely by interpolating at `resolution` (0.05 rad L2 by default); the robot predicate calls MuJoCo `mj_collision`, and Demo 3's carry also calls explicit distance queries for three payload/environment pairs. This is sampled collision checking, not continuous certification. Greedy then random shortcutting removes redundant waypoints; the path is densified to the same resolution for execution. Sampling uses the in-repo SplitMix64 RNG — same seed, same path.
 - **Collision filter.** Only contacts that involve a robot collision geom (`contype ≠ 0`, attached to a chain body, not the world) count. Floor-vs-pillar contacts are ignored; parent–child pairs are already excluded by MuJoCo. Visual meshes never participate.
+- **Pair-scoped attached load.** `AttachedBoxCollisionChecker` solves the mocap-body pose so the compiled proxy geom equals `T_world_EE * T_EE_proxy`, including nonzero geom-local offsets. It queries only constructor-validated environment names and rejects signed distances `< 0.005 m` in Demo 3. Robot and place-pad geoms are excluded from this scope; no global positive robot threshold is applied.
 - **Deterministic.** Restart sampling, RRT sampling, and random shortcutting all use the in-repo RNG with a fixed seed. The timed trajectory is bit-stable given the same seed and limits (CI golden test).
 - **Scalar S-curve time law.** A rest-to-rest 7-phase bang-bang-jerk profile times the scalar path length `s ∈ [0, L]`. Per-joint `(v, a, j)` limits are converted to path-space limits by the steepest `|dqᵢ/ds|` on the polyline, so no joint exceeds its bound within an edge. Polyline tangent discontinuities are not blended: joint velocity can jump at a corner, so the complete joint trajectory is not globally acceleration- or jerk-bounded.
-- **Physics-side servo.** The demos apply the exact MuJoCo bias force as gravity/Coriolis feedforward. Position actuators are commanded as a PD tracker with velocity feedforward: `ctrl = q_des + (kv/kp)·qd_des` yields `τ = kp(q_des − q) + kv(qd_des − qd)`. Worst joint-space tracking is 0.0046 rad (Demo 2) and 0.0037 rad (Demo 3).
-- **Scripted grasp.** Demo 3 welds a mocap cube to the EE after the pick descend and parks it on the place pad after the place descend. That is a kinematics/planning demo, not contact-rich grasping. The cube volume is not represented in the planner collision geometry, and carry planning uses a zero contact threshold that only rejects sampled robot penetration. Attached-load and pair-scoped clearance checks remain future work.
+- **Physics-side servo.** The demos apply the exact MuJoCo bias force as gravity/Coriolis feedforward. Position actuators are commanded as a PD tracker with velocity feedforward: `ctrl = q_des + (kv/kp)·qd_des` yields `τ = kp(q_des − q) + kv(qd_des − qd)`. Worst joint-space tracking is 0.0046 rad (Demo 2) and 0.0067 rad in the representative attached-load Demo 3 run.
+- **Scripted grasp.** Demo 3 welds a mocap cube to the EE after the pick descend and parks it on the place pad after the place descend. During carry, that same compiled box is a 5 mm pair-scoped planning proxy against floor, table, and pillar. This remains a kinematics/planning demo, not contact-rich grasping: it does not model slip, fingers, compliance, grasp uncertainty, continuous swept volume, calibration error, or cube-versus-robot self-contact.
 
 ## Roadmap
 
@@ -107,6 +115,7 @@ Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PAT
 - [x] Pick-and-place with obstacle dodging; benchmark tables
 - [x] Reproducible controller robustness matrix with raw CSV and explicit sim-only limits
 - [x] Multi-scene, multi-query planning and tracking extension with raw CSVs
+- [x] Pair-scoped attached-cube collision proxy for Demo 3 carry
 
 ## License & assets
 

crates/arm-lab-demo/src/bin/demo3.rs

@@ -1,9 +1,10 @@
 //! Demo 3 — pick-and-place around a pillar, from scratch.
 //!
 //! IK solves grasp poses, RRT-Connect carries the cube around a pillar that
-//! blocks the joint-space interpolant, a jerk-bounded scalar S-curve times every
-//! segment, and a mocap weld stands in for a gripper (scripted attach, not
-//! contact-rich grasping).
+//! blocks the joint-space interpolant, a pair-scoped attached-box proxy checks
+//! the carried cube against the environment, a jerk-bounded scalar S-curve
+//! times every segment, and a mocap weld stands in for a gripper (scripted
+//! attach, not contact-rich grasping).
 //!
 //! ```text
 //! cargo run --release -p arm-lab-demo --bin demo3
@@ -16,7 +17,9 @@ use arm_lab::ik::{IkConfig, solve_ik};
 use arm_lab::kinematics::fk;
 use arm_lab::plan::rrt_connect;
 use arm_lab::traj::{TrajLimits, time_parameterize};
-use arm_lab::{Chain, CollisionChecker, PlanConfig, PlanStatus};
+use arm_lab::{
+    AttachedBoxCollisionChecker, AttachedBoxSpec, Chain, CollisionChecker, PlanConfig, PlanStatus,
+};
 use arm_lab_demo::{
     GIF_FPS, RENDER_EVERY, RENDER_H, RENDER_W, capture_frame, encode_gif, gravity_compensate,
     init_recording, log_transform, parse_args, read_q, set_ctrl, traj_step,
@@ -53,10 +56,10 @@ const KV_OVER_KP: f64 = 0.2;
 const LOG_EVERY: usize = 2;
 const SETTLE_STEPS: usize = 80;
 /// Carry planning rejects sampled robot penetration.
-///
-/// The carried cube is not represented in the planner collision geometry, so
-/// this does not certify clearance for the attached load.
 const CARRY_CONTACT_THRESHOLD: f64 = 0.0;
+/// Pair-scoped positive planning buffer for the attached cube only.
+const PAYLOAD_CLEARANCE_M: f64 = 0.005;
+const PAYLOAD_ENVIRONMENT: [&str; 3] = ["floor", "table", "pillar"];
 const SEED: u64 = 20260816;
 
 fn main() {
@@ -86,6 +89,18 @@ fn main() {
     };
     let mut cc = CollisionChecker::new(&model, &chain);
     assert!(!cc.collides(&q_home), "home is in collision");
+    let mut carry_cc = AttachedBoxCollisionChecker::new(
+        &model,
+        &chain,
+        AttachedBoxSpec::new(
+            "cube",
+            Isometry3::translation(CUBE_IN_EE.x, CUBE_IN_EE.y, CUBE_IN_EE.z),
+            PAYLOAD_ENVIRONMENT,
+            PAYLOAD_CLEARANCE_M,
+        ),
+    )
+    .expect("valid attached cube collision proxy");
+    carry_cc.set_robot_contact_threshold(CARRY_CONTACT_THRESHOLD);
 
     let q_pick_app = solve_named(
         "pick_approach",
@@ -216,14 +231,13 @@ fn main() {
         &q_pick_app,
         1e-3,
     );
-    q = go(
+    q = go_with_collision(
         "carry around pillar",
         &mut ctx,
-        &mut cc,
         &plan_cfg,
         &q,
         &q_place_app,
-        CARRY_CONTACT_THRESHOLD,
+        &mut |candidate| carry_cc.collides(candidate),
     );
     q = go(
         "descend to place",
@@ -300,8 +314,19 @@ fn go(
 ) -> Vec<f64> {
     cc.contact_threshold = contact_threshold;
     let mut collides = |q: &[f64]| cc.collides(q);
+    go_with_collision(name, ctx, plan_cfg, q_from, q_to, &mut collides)
+}
+
+fn go_with_collision(
+    name: &str,
+    ctx: &mut ExecCtx<'_, '_>,
+    plan_cfg: &PlanConfig,
+    q_from: &[f64],
+    q_to: &[f64],
+    collides: &mut impl FnMut(&[f64]) -> bool,
+) -> Vec<f64> {
     let t0 = std::time::Instant::now();
-    let plan = rrt_connect(ctx.chain, q_from, q_to, &mut collides, plan_cfg);
+    let plan = rrt_connect(ctx.chain, q_from, q_to, collides, plan_cfg);
     let plan_ms = t0.elapsed().as_secs_f64() * 1e3;
     assert_eq!(
         plan.status,