Add multi-scene planning and penetration-aware tracking evidence

.github/workflows/ci.yml

@@ -38,6 +38,8 @@ jobs:
         run: |
           export LD_LIBRARY_PATH="$MUJOCO_DOWNLOAD_DIR/mujoco-3.9.0/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
           cargo test --release -p arm-lab
+          cargo test --release -p arm-lab-demo --bin multi_query_bench
+          cargo run --release -p arm-lab-demo --bin multi_query_bench -- --check
 
       - name: Build demo (no GL runtime needed to compile)
         run: cargo build --release -p arm-lab-demo

README.md

@@ -35,6 +35,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.
 
+## 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.
+
+Position PD meets the numeric gates in **0/18** tracking cases. Desired-velocity feedforward meets the numeric gates in **14/18** cases and passes the full zero-penetration gate in **13/18**. The four sampled-penetration cases are all the retained `reverse_cross_workspace` negative: 25--72 path steps depending on plant/controller, with 0.050--0.079 mm maximum actual penetration; settling and hold remain penetration-free. The generated [report](docs/multi_query_results.md), [45-row planning CSV](docs/multi_query_planning.csv), and [36-row tracking CSV](docs/multi_query_tracking.csv) retain every outcome and include exact joint vectors, seeds, trajectory metrics, per-phase penetration counts/depths and contact identities, pass criteria, and claim boundaries. The fixtures are hand-designed and deterministic, not a sampled task distribution; the results do not estimate hardware or workspace-wide success probability.
+
 ## Layout
 
 ```
@@ -72,6 +78,12 @@ cargo run --release -p arm-lab-demo --bin demo3 -- --connect
 
 # deterministic controller-ablation × plant-shift matrix; write Markdown + CSV
 cargo run --release -p arm-lab-demo --bin robustness_bench -- --write
+
+# three scenes × three fixed queries; write planning/tracking CSVs + report
+cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write
+
+# rerun the bounded matrix and verify deterministic committed fields/outcomes
+cargo run --release -p arm-lab-demo --bin multi_query_bench -- --check
 ```
 
 Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PATH. On Linux without system MuJoCo, `mujoco-rs` auto-downloads MuJoCo 3.9 at build time. Set `MUJOCO_DOWNLOAD_DIR` to an absolute directory before building and add its downloaded `lib/` directory to `LD_LIBRARY_PATH` before running; see the [mujoco-rs docs](https://github.com/davidhozic/mujoco-rs).
@@ -85,7 +97,7 @@ Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PAT
 - **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 carry query inflates collision clearance by 4 cm so the cube volume clears the pillar.
+- **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.
 
 ## Roadmap
 
@@ -94,6 +106,7 @@ Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PAT
 - [x] Jerk-bounded scalar S-curve time law; joint-space PD + velocity feedforward
 - [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
 
 ## License & assets
 

assets/ur5e/scene_pickplace.xml

@@ -45,13 +45,9 @@
     </body>
 
     <!--
-      Mocap cube: the demo welds it to the EE after pick and parks it on the
-      pad after place. Visual + a thin collision so the planner treats the
-      resting cube as an obstacle at the pick pose, but the weld in the demo
-      keeps it from generating EE-vs-cube contacts (contype is left on; the
-      checker only flags robot-involved contacts, and we disable the cube's
-      contype from the demo's live model if needed). Default: no collision,
-      so the descend-to-pick does not fight the cube geom.
+      Visual/mocap-only cube: the demo welds it to the EE after pick and parks
+      it on the pad after place. Its contact masks are zero, so it is excluded
+      from planner collision geometry.
     -->
     <body name="cube" mocap="true" pos="-0.24 0.58 0.385">
       <geom name="cube" type="box" size="0.025 0.025 0.025" material="cube"

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

@@ -52,8 +52,11 @@ const CUBE_REST_PLACE: [f64; 3] = [0.22, 0.58, 0.385];
 const KV_OVER_KP: f64 = 0.2;
 const LOG_EVERY: usize = 2;
 const SETTLE_STEPS: usize = 80;
-/// Inflate obstacles during the carry so the cube volume clears the pillar.
-const CARRY_CLEARANCE: f64 = 0.04;
+/// 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;
 const SEED: u64 = 20260816;
 
 fn main() {
@@ -220,7 +223,7 @@ fn main() {
         &plan_cfg,
         &q,
         &q_place_app,
-        CARRY_CLEARANCE,
+        CARRY_CONTACT_THRESHOLD,
     );
     q = go(
         "descend to place",
@@ -293,9 +296,9 @@ fn go(
     plan_cfg: &PlanConfig,
     q_from: &[f64],
     q_to: &[f64],
-    clearance: f64,
+    contact_threshold: f64,
 ) -> Vec<f64> {
-    cc.clearance = clearance;
+    cc.contact_threshold = contact_threshold;
     let mut collides = |q: &[f64]| cc.collides(q);
     let t0 = std::time::Instant::now();
     let plan = rrt_connect(ctx.chain, q_from, q_to, &mut collides, plan_cfg);