diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3170775..be4befb 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/README.md b/README.md index 0f4fd9e..2821142 100644 --- a/README.md +++ b/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 diff --git a/assets/ur5e/scene_pickplace.xml b/assets/ur5e/scene_pickplace.xml index 94b5ed1..57144cb 100644 --- a/assets/ur5e/scene_pickplace.xml +++ b/assets/ur5e/scene_pickplace.xml @@ -45,13 +45,9 @@ Vec { - 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); diff --git a/crates/arm-lab-demo/src/bin/multi_query_bench.rs b/crates/arm-lab-demo/src/bin/multi_query_bench.rs new file mode 100644 index 0000000..e3a810d --- /dev/null +++ b/crates/arm-lab-demo/src/bin/multi_query_bench.rs @@ -0,0 +1,1206 @@ +//! Deterministic multi-scene, multi-query integration benchmark. +//! +//! This executable complements, rather than replaces, the single-trajectory +//! controller robustness envelope. It evaluates five planner seeds on nine +//! fixed scene-query fixtures (six unique joint-pair definitions) across all +//! three shipped UR5e scenes. The canonical-seed trajectory for every fixture +//! is then replayed with position PD and with position PD plus desired-velocity +//! feedforward, both on the nominal plant and on the same fixed combined plant +//! shift used by `robustness_bench`. +//! +//! ```text +//! cargo run --release -p arm-lab-demo --bin multi_query_bench +//! cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write +//! cargo run --release -p arm-lab-demo --bin multi_query_bench -- --check +//! ``` + +use std::collections::VecDeque; +use std::fmt::Write as _; +use std::path::Path; + +use arm_lab::plan::{PlanStatus, edge_free, rrt_connect}; +use arm_lab::traj::{TrajLimits, Trajectory, time_parameterize}; +use arm_lab::{Chain, CollisionChecker, PlanConfig}; +use arm_lab_demo::{read_q, set_ctrl}; +use mujoco_rs::prelude::*; + +const ASSET_DIR: &str = concat!(env!("CARGO_MANIFEST_DIR"), "/../../assets/ur5e/"); +const ACTUATORS: [&str; 6] = [ + "shoulder_pan", + "shoulder_lift", + "elbow", + "wrist_1", + "wrist_2", + "wrist_3", +]; +const JOINTS: [&str; 6] = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +]; + +const CANONICAL_SEED: u64 = 20260816; +const SEEDS: [u64; 5] = [20260816, 20260817, 20260818, 20260819, 20260820]; +const KV_OVER_KP: f64 = 0.2; +const SETTLE_STEPS: usize = 250; +const HOLD_STEPS: usize = 250; +const PASS_RMS_RAD: f64 = 0.03; +const PASS_MAX_RAD: f64 = 0.10; +const PASS_FINAL_RAD: f64 = 0.02; +/// Execution audit counts only emitted robot contacts with signed distance +/// below zero: sampled geometric penetration, not a positive clearance claim. +const EXECUTION_CONTACT_THRESHOLD_M: f64 = 0.0; +const LIMITS: TrajLimits = TrajLimits { + v_max: 0.55, + a_max: 1.8, + j_max: 8.0, +}; + +#[derive(Clone, Copy)] +struct Scene { + name: &'static str, + file: &'static str, +} + +const SCENES: [Scene; 3] = [ + Scene { + name: "open_floor", + file: "scene.xml", + }, + Scene { + name: "offset_pillar", + file: "scene_cluttered.xml", + }, + Scene { + name: "tabletop_pillar", + file: "scene_pickplace.xml", + }, +]; + +#[derive(Clone, Copy)] +struct Query { + scene: usize, + name: &'static str, + start_delta: [f64; 6], + goal_delta: [f64; 6], +} + +// Hand-designed, fixed joint-space fixtures. Three queries have obstructed +// straight interpolants: offset_pillar/positive_pan and both cross-workspace +// queries in tabletop_pillar. All endpoints are collision-free by assertion. +const QUERIES: [Query; 9] = [ + Query { + scene: 0, + name: "positive_pan", + start_delta: [0.0; 6], + goal_delta: [1.10, 0.0, 0.0, 0.0, 0.0, 0.0], + }, + Query { + scene: 0, + name: "shoulder_elbow", + start_delta: [0.0; 6], + goal_delta: [0.35, 0.40, -0.55, 0.25, 0.0, 0.0], + }, + Query { + scene: 0, + name: "wrist_reorientation", + start_delta: [0.0; 6], + goal_delta: [0.25, -0.15, 0.25, -0.35, 0.60, -0.70], + }, + Query { + scene: 1, + name: "positive_pan", + start_delta: [0.0; 6], + goal_delta: [1.10, 0.0, 0.0, 0.0, 0.0, 0.0], + }, + Query { + scene: 1, + name: "negative_pan", + start_delta: [0.0; 6], + goal_delta: [-1.00, 0.0, 0.0, 0.0, 0.0, 0.0], + }, + Query { + scene: 1, + name: "shoulder_elbow", + start_delta: [0.0; 6], + goal_delta: [0.35, 0.40, -0.55, 0.25, 0.0, 0.0], + }, + Query { + scene: 2, + name: "cross_workspace", + start_delta: [-0.40, 0.15, -0.20, 0.10, 0.0, 0.0], + goal_delta: [0.75, 0.25, -0.45, 0.25, 0.15, -0.20], + }, + Query { + scene: 2, + name: "reverse_cross_workspace", + start_delta: [0.65, 0.20, -0.35, 0.15, 0.10, -0.20], + goal_delta: [-0.45, 0.10, -0.10, -0.10, -0.20, 0.35], + }, + Query { + scene: 2, + name: "wrist_reorientation", + start_delta: [0.0; 6], + goal_delta: [0.25, -0.15, 0.25, -0.35, 0.60, -0.70], + }, +]; + +#[derive(Clone, Copy)] +struct PlantScenario { + name: &'static str, + payload_kg: f64, + actuator_scale: f64, + extra_damping: f64, + delay_ms: f64, + pulse_nm: f64, +} + +const PLANTS: [PlantScenario; 2] = [ + PlantScenario { + name: "nominal", + payload_kg: 0.0, + actuator_scale: 1.0, + extra_damping: 0.0, + delay_ms: 0.0, + pulse_nm: 0.0, + }, + PlantScenario { + name: "combined moderate shift", + payload_kg: 1.0, + actuator_scale: 0.80, + extra_damping: 1.0, + delay_ms: 10.0, + pulse_nm: 10.0, + }, +]; + +#[derive(Clone, Copy)] +enum Controller { + Position, + VelocityFf, +} + +const CONTROLLERS: [Controller; 2] = [Controller::Position, Controller::VelocityFf]; + +impl Controller { + fn name(self) -> &'static str { + match self { + Self::Position => "position PD", + Self::VelocityFf => "PD + velocity FF", + } + } + + fn velocity_ff(self) -> bool { + matches!(self, Self::VelocityFf) + } +} + +struct PlanningRow { + scene: &'static str, + scene_file: &'static str, + query: &'static str, + seed: u64, + start: Vec, + goal: Vec, + direct_free: bool, + status: PlanStatus, + elapsed_ms: f64, + iterations: usize, + nodes: usize, + shortcut_waypoints: usize, + path_samples: usize, + path_cost_rad: f64, + trajectory_samples: usize, + trajectory_duration_s: f64, +} + +#[derive(Clone)] +struct TrackingRow { + scene: &'static str, + scene_file: &'static str, + query: &'static str, + direct_free: bool, + plant: &'static str, + controller: &'static str, + trajectory_samples: usize, + trajectory_duration_s: f64, + rms_joint_rad: f64, + max_joint_rad: f64, + final_joint_rad: f64, + max_ee_pos_m: f64, + peak_force_fraction: f64, + saturated_step_fraction: f64, + settle_collisions: CollisionPhaseMetrics, + path_collisions: CollisionPhaseMetrics, + hold_collisions: CollisionPhaseMetrics, + pass: bool, +} + +impl TrackingRow { + fn penetration_steps(&self) -> usize { + self.settle_collisions.steps + self.path_collisions.steps + self.hold_collisions.steps + } + + fn max_penetration_m(&self) -> f64 { + self.settle_collisions + .max_penetration_m + .max(self.path_collisions.max_penetration_m) + .max(self.hold_collisions.max_penetration_m) + } +} + +#[derive(Clone, Default)] +struct CollisionPhaseMetrics { + steps: usize, + max_penetration_m: f64, + worst_contact_distance_m: Option, + worst_contact: Option, +} + +impl CollisionPhaseMetrics { + fn observe(&mut self, checker: &mut CollisionChecker<&MjModel>, q: &[f64]) { + let contacts = checker.robot_contacts(q); + if contacts.is_empty() { + return; + } + self.steps += 1; + for contact in contacts { + let penetration = (-contact.distance_m).max(0.0); + if penetration > self.max_penetration_m { + self.max_penetration_m = penetration; + self.worst_contact_distance_m = Some(contact.distance_m); + self.worst_contact = Some(contact.identity()); + } + } + } +} + +#[derive(Clone, Copy, PartialEq, Eq)] +enum Mode { + Run, + Write, + Check, +} + +fn parse_mode() -> Mode { + let args: Vec = std::env::args().skip(1).collect(); + match args.as_slice() { + [] => Mode::Run, + [arg] if arg == "--write" => Mode::Write, + [arg] if arg == "--check" => Mode::Check, + _ => panic!("usage: multi_query_bench [--write|--check]"), + } +} + +struct DelayedCommand { + ctrl: Vec, +} + +fn main() { + let mode = parse_mode(); + let mut planning_rows = Vec::with_capacity(QUERIES.len() * SEEDS.len()); + let mut tracking_rows = Vec::with_capacity(QUERIES.len() * PLANTS.len() * CONTROLLERS.len()); + + for (scene_index, scene) in SCENES.iter().copied().enumerate() { + let nominal_model = load_model(scene); + let nominal_chain = extract_chain(&nominal_model); + let home = home_configuration(&nominal_model, &nominal_chain); + + for query in QUERIES.iter().copied().filter(|q| q.scene == scene_index) { + let start = offset(&home, query.start_delta); + let goal = offset(&home, query.goal_delta); + assert_within_limits(&nominal_chain, &start, "start", scene, query); + assert_within_limits(&nominal_chain, &goal, "goal", scene, query); + + let mut collision = CollisionChecker::new(&nominal_model, &nominal_chain); + assert!( + !collision.collides(&start), + "{}/{} start is in collision", + scene.name, + query.name + ); + assert!( + !collision.collides(&goal), + "{}/{} goal is in collision", + scene.name, + query.name + ); + let mut scratch = vec![0.0; nominal_chain.dof()]; + let direct_free = edge_free( + &start, + &goal, + PlanConfig::default().resolution, + &mut |q| collision.collides(q), + &mut scratch, + ); + + let mut canonical_trajectory = None; + for seed in SEEDS { + let mut collision = CollisionChecker::new(&nominal_model, &nominal_chain); + let plan = rrt_connect( + &nominal_chain, + &start, + &goal, + |q| collision.collides(q), + &PlanConfig { + seed, + ..PlanConfig::default() + }, + ); + assert_eq!( + plan.status, + PlanStatus::Success, + "{}/{} failed at seed {}", + scene.name, + query.name, + seed + ); + let trajectory = + time_parameterize(&plan.path, &LIMITS, nominal_model.opt().timestep); + if seed == CANONICAL_SEED { + canonical_trajectory = Some(trajectory.clone()); + } + planning_rows.push(PlanningRow { + scene: scene.name, + scene_file: scene.file, + query: query.name, + seed, + start: start.clone(), + goal: goal.clone(), + direct_free, + status: plan.status, + elapsed_ms: 1e3 * plan.elapsed_s, + iterations: plan.iterations, + nodes: plan.nodes, + shortcut_waypoints: plan.waypoints.len(), + path_samples: plan.path.len(), + path_cost_rad: plan.cost, + trajectory_samples: trajectory.len(), + trajectory_duration_s: trajectory.duration, + }); + } + + let trajectory = canonical_trajectory.expect("canonical-seed trajectory"); + println!( + "{:<18} {:<24} direct={} · {} samples / {:.2} s", + scene.name, + query.name, + if direct_free { "free" } else { "blocked" }, + trajectory.len(), + trajectory.duration + ); + + for plant_scenario in PLANTS { + let plant_model = perturbed_model(scene, plant_scenario); + let plant_chain = extract_chain(&plant_model); + for controller in CONTROLLERS { + let metrics = run_tracking_case( + scene, + query, + direct_free, + plant_scenario, + controller, + &plant_model, + &plant_chain, + &trajectory, + ); + println!( + " {:<23} | {:<16} | rms {:>7.4} | max {:>7.4} | final {:>7.4} | penetration steps {:>4} | pen {:>7.3} mm | {}", + metrics.plant, + metrics.controller, + metrics.rms_joint_rad, + metrics.max_joint_rad, + metrics.final_joint_rad, + metrics.penetration_steps(), + 1e3 * metrics.max_penetration_m(), + if metrics.pass { "PASS" } else { "FAIL" } + ); + tracking_rows.push(metrics); + } + } + } + } + + let blocked_queries = planning_rows + .iter() + .filter(|row| row.seed == CANONICAL_SEED && !row.direct_free) + .count(); + let direct_trials = planning_rows.iter().filter(|row| row.direct_free).count(); + let direct_successes = planning_rows + .iter() + .filter(|row| row.direct_free && row.status == PlanStatus::Success) + .count(); + let obstructed_trials = planning_rows.iter().filter(|row| !row.direct_free).count(); + let obstructed_successes = planning_rows + .iter() + .filter(|row| !row.direct_free && row.status == PlanStatus::Success) + .count(); + let velocity_passes = tracking_rows + .iter() + .filter(|row| row.controller == Controller::VelocityFf.name() && row.pass) + .count(); + println!( + "summary: direct-free {direct_successes}/{direct_trials}; obstructed {obstructed_successes}/{obstructed_trials}; {blocked_queries}/{} blocked-direct fixtures; {velocity_passes}/{} velocity-FF passes", + QUERIES.len(), + QUERIES.len() * PLANTS.len() + ); + + match mode { + Mode::Run => {} + Mode::Write => write_artifacts(&planning_rows, &tracking_rows), + Mode::Check => check_artifacts(&planning_rows, &tracking_rows), + } +} + +fn load_model(scene: Scene) -> MjModel { + let path = format!("{ASSET_DIR}{}", scene.file); + MjModel::from_xml(&path).unwrap_or_else(|error| panic!("load {}: {error}", scene.file)) +} + +fn extract_chain(model: &MjModel) -> Chain { + Chain::from_mujoco(model, "ur5e", "wrist_3_link", "attachment_site") + .expect("extract UR5e chain") +} + +fn home_configuration(model: &MjModel, chain: &Chain) -> Vec { + let mut data = MjData::new(model); + let home_key = model + .name_to_id(MjtObj::mjOBJ_KEY, "home") + .expect("home keyframe"); + data.reset_keyframe(home_key).expect("reset to home"); + data.forward(); + read_q(&data, chain) +} + +fn offset(home: &[f64], delta: [f64; 6]) -> Vec { + home.iter().zip(delta).map(|(q, dq)| q + dq).collect() +} + +fn assert_within_limits(chain: &Chain, q: &[f64], endpoint: &str, scene: Scene, query: Query) { + for (index, (&value, limits)) in q.iter().zip(chain.joint_limits()).enumerate() { + if let Some((lower, upper)) = limits { + assert!( + (lower..=upper).contains(&value), + "{}/{} {} joint {} is outside [{}, {}]", + scene.name, + query.name, + endpoint, + index, + lower, + upper + ); + } + } +} + +fn perturbed_model(scene: Scene, scenario: PlantScenario) -> MjModel { + let path = format!("{ASSET_DIR}{}", scene.file); + let mut spec = MjSpec::from_xml(&path).expect("load editable UR5e scene"); + + if scenario.payload_kg > 0.0 { + let payload = spec + .body_mut("wrist_3_link") + .expect("wrist body") + .add_body(); + payload + .set_name("multi_query_payload") + .expect("valid payload name"); + payload.set_explicitinertial(true); + payload.set_mass(scenario.payload_kg); + payload.pos_mut().copy_from_slice(&[0.0, 0.10, 0.0]); + let inertia = 0.0017 * scenario.payload_kg; + payload + .inertia_mut() + .copy_from_slice(&[inertia, inertia, inertia]); + } + + if scenario.extra_damping > 0.0 { + for name in JOINTS { + spec.joint_mut(name).expect("benchmark joint").damping_mut()[0] += + scenario.extra_damping; + } + } + + if scenario.actuator_scale != 1.0 { + for name in ACTUATORS { + let actuator = spec.actuator_mut(name).expect("benchmark actuator"); + actuator.gainprm_mut()[0] *= scenario.actuator_scale; + actuator.biasprm_mut()[1] *= scenario.actuator_scale; + actuator.biasprm_mut()[2] *= scenario.actuator_scale; + } + } + + spec.compile().expect("compile perturbed UR5e model") +} + +#[allow(clippy::too_many_arguments)] +fn run_tracking_case( + scene: Scene, + query: Query, + direct_free: bool, + scenario: PlantScenario, + controller: Controller, + model: &MjModel, + chain: &Chain, + trajectory: &Trajectory, +) -> TrackingRow { + let mut data = MjData::new(model); + let home_key = model + .name_to_id(MjtObj::mjOBJ_KEY, "home") + .expect("home keyframe"); + data.reset_keyframe(home_key).expect("reset to home"); + + let q_start = trajectory.q.first().expect("trajectory start"); + for (&address, &value) in chain.qpos_addresses().iter().zip(q_start) { + data.qpos_mut()[address] = value; + } + for address in chain.dof_addresses() { + data.qvel_mut()[address] = 0.0; + } + data.forward(); + + let dt = model.opt().timestep; + let delay_steps = (scenario.delay_ms * 1e-3 / dt).round() as usize; + let zero = vec![0.0; chain.dof()]; + let mut queue = VecDeque::with_capacity(delay_steps + 1); + let mut collision_checker = CollisionChecker::new(model, chain); + collision_checker.contact_threshold = EXECUTION_CONTACT_THRESHOLD_M; + let mut settle_collisions = CollisionPhaseMetrics::default(); + let mut path_collisions = CollisionPhaseMetrics::default(); + let mut hold_collisions = CollisionPhaseMetrics::default(); + for _ in 0..delay_steps { + queue.push_back(DelayedCommand { + ctrl: q_start.clone(), + }); + } + + for _ in 0..SETTLE_STEPS { + control_step( + controller, scenario, &mut data, chain, q_start, &zero, &mut queue, false, + ); + settle_collisions.observe(&mut collision_checker, &read_q(&data, chain)); + } + + let mut sum_sq = 0.0; + let mut samples = 0usize; + let mut max_joint = 0.0f64; + let mut max_ee = 0.0f64; + let mut peak_force_fraction = 0.0f64; + let mut saturated_steps = 0usize; + let pulse_start = trajectory.len() * 45 / 100; + let pulse_steps = (0.120 / dt).round() as usize; + + for (index, (q_des, qd_des)) in trajectory.q.iter().zip(&trajectory.qd).enumerate() { + let pulse = + scenario.pulse_nm != 0.0 && (pulse_start..pulse_start + pulse_steps).contains(&index); + control_step( + controller, scenario, &mut data, chain, q_des, qd_des, &mut queue, pulse, + ); + + let q_measured = read_q(&data, chain); + path_collisions.observe(&mut collision_checker, &q_measured); + let error = l2(&q_measured, q_des); + sum_sq += error * error; + samples += 1; + max_joint = max_joint.max(error); + let ee_error = (arm_lab::kinematics::fk(chain, &q_measured) + .translation + .vector + - arm_lab::kinematics::fk(chain, q_des).translation.vector) + .norm(); + max_ee = max_ee.max(ee_error); + let fraction = force_fraction(model, &data); + peak_force_fraction = peak_force_fraction.max(fraction); + saturated_steps += usize::from(fraction >= 0.999); + } + + let q_goal = trajectory.q.last().expect("trajectory goal"); + for _ in 0..HOLD_STEPS { + control_step( + controller, scenario, &mut data, chain, q_goal, &zero, &mut queue, false, + ); + hold_collisions.observe(&mut collision_checker, &read_q(&data, chain)); + } + let final_joint = l2(&read_q(&data, chain), q_goal); + let rms_joint = (sum_sq / samples as f64).sqrt(); + let penetration_steps = settle_collisions.steps + path_collisions.steps + hold_collisions.steps; + let pass = case_pass(rms_joint, max_joint, final_joint, penetration_steps); + + TrackingRow { + scene: scene.name, + scene_file: scene.file, + query: query.name, + direct_free, + plant: scenario.name, + controller: controller.name(), + trajectory_samples: trajectory.len(), + trajectory_duration_s: trajectory.duration, + rms_joint_rad: rms_joint, + max_joint_rad: max_joint, + final_joint_rad: final_joint, + max_ee_pos_m: max_ee, + peak_force_fraction, + saturated_step_fraction: saturated_steps as f64 / samples as f64, + settle_collisions, + path_collisions, + hold_collisions, + pass, + } +} + +#[allow(clippy::too_many_arguments)] +fn control_step( + controller: Controller, + scenario: PlantScenario, + data: &mut MjData<&MjModel>, + chain: &Chain, + q_des: &[f64], + qd_des: &[f64], + queue: &mut VecDeque, + pulse: bool, +) { + let mut ctrl = q_des.to_vec(); + if controller.velocity_ff() { + for (command, velocity) in ctrl.iter_mut().zip(qd_des) { + *command += KV_OVER_KP * velocity; + } + } + queue.push_back(DelayedCommand { ctrl }); + let command = queue.pop_front().expect("delayed command"); + + set_ctrl(data, &ACTUATORS, &command.ctrl); + data.qfrc_applied_mut().fill(0.0); + if pulse { + data.qfrc_applied_mut()[chain.dof_addresses()[1]] -= scenario.pulse_nm; + } + data.step(); +} + +fn force_fraction(model: &MjModel, data: &MjData<&MjModel>) -> f64 { + data.actuator_force() + .iter() + .zip(model.actuator_forcerange()) + .map(|(&force, range)| { + let limit = range[0].abs().max(range[1].abs()); + if limit > 0.0 { + force.abs() / limit + } else { + 0.0 + } + }) + .fold(0.0, f64::max) +} + +fn l2(a: &[f64], b: &[f64]) -> f64 { + a.iter() + .zip(b) + .map(|(x, y)| (x - y).powi(2)) + .sum::() + .sqrt() +} + +fn numeric_gates_pass(row: &TrackingRow) -> bool { + row.rms_joint_rad <= PASS_RMS_RAD + && row.max_joint_rad <= PASS_MAX_RAD + && row.final_joint_rad <= PASS_FINAL_RAD +} + +fn case_pass(rms_joint: f64, max_joint: f64, final_joint: f64, penetration_steps: usize) -> bool { + rms_joint <= PASS_RMS_RAD + && max_joint <= PASS_MAX_RAD + && final_joint <= PASS_FINAL_RAD + && penetration_steps == 0 +} + +fn worst_collision(row: &TrackingRow) -> (&'static str, &CollisionPhaseMetrics) { + [ + ("settle", &row.settle_collisions), + ("path", &row.path_collisions), + ("hold", &row.hold_collisions), + ] + .into_iter() + .max_by(|(_, left), (_, right)| left.max_penetration_m.total_cmp(&right.max_penetration_m)) + .expect("three collision phases") +} + +fn format_optional_distance(value: Option) -> String { + value.map_or_else(String::new, |distance| format!("{distance:.8}")) +} + +fn status_name(status: PlanStatus) -> &'static str { + match status { + PlanStatus::Success => "success", + PlanStatus::StartCollision => "start_collision", + PlanStatus::GoalCollision => "goal_collision", + PlanStatus::Unconnected => "unconnected", + } +} + +fn format_joint_vector(q: &[f64]) -> String { + q.iter() + .map(|value| format!("{value:.8}")) + .collect::>() + .join(";") +} + +struct Artifacts { + planning_csv: String, + tracking_csv: String, + report: String, +} + +fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artifacts { + let mut planning_csv = String::from( + "scene,scene_file,query,seed,start_q_rad,goal_q_rad,direct_path_free,status,plan_elapsed_ms,iterations,nodes,shortcut_waypoints,path_samples,path_cost_rad,trajectory_samples,trajectory_duration_s\n", + ); + for row in planning { + writeln!( + planning_csv, + "{},{},{},{},{},{},{},{},{:.8},{},{},{},{},{:.8},{},{:.8}", + row.scene, + row.scene_file, + row.query, + row.seed, + format_joint_vector(&row.start), + format_joint_vector(&row.goal), + row.direct_free, + status_name(row.status), + row.elapsed_ms, + row.iterations, + row.nodes, + row.shortcut_waypoints, + row.path_samples, + row.path_cost_rad, + row.trajectory_samples, + row.trajectory_duration_s + ) + .expect("format planning CSV"); + } + let mut tracking_csv = String::from( + "scene,scene_file,query,seed,direct_path_free,plant,controller,trajectory_samples,trajectory_duration_s,rms_joint_rad,max_joint_rad,final_joint_rad,max_ee_pos_m,peak_force_fraction,saturated_step_fraction,settle_penetration_steps,settle_max_penetration_m,settle_worst_contact_distance_m,settle_worst_contact,path_penetration_steps,path_max_penetration_m,path_worst_contact_distance_m,path_worst_contact,hold_penetration_steps,hold_max_penetration_m,hold_worst_contact_distance_m,hold_worst_contact,penetration_steps,max_penetration_m,pass\n", + ); + for row in tracking { + writeln!( + tracking_csv, + "{},{},{},{},{},{},{},{},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{},{:.8},{},{},{},{:.8},{},{},{},{:.8},{},{},{},{:.8},{}", + row.scene, + row.scene_file, + row.query, + CANONICAL_SEED, + row.direct_free, + row.plant, + row.controller, + row.trajectory_samples, + row.trajectory_duration_s, + row.rms_joint_rad, + row.max_joint_rad, + row.final_joint_rad, + row.max_ee_pos_m, + row.peak_force_fraction, + row.saturated_step_fraction, + row.settle_collisions.steps, + row.settle_collisions.max_penetration_m, + format_optional_distance(row.settle_collisions.worst_contact_distance_m), + row.settle_collisions.worst_contact.as_deref().unwrap_or(""), + row.path_collisions.steps, + row.path_collisions.max_penetration_m, + format_optional_distance(row.path_collisions.worst_contact_distance_m), + row.path_collisions.worst_contact.as_deref().unwrap_or(""), + row.hold_collisions.steps, + row.hold_collisions.max_penetration_m, + format_optional_distance(row.hold_collisions.worst_contact_distance_m), + row.hold_collisions.worst_contact.as_deref().unwrap_or(""), + row.penetration_steps(), + row.max_penetration_m(), + row.pass + ) + .expect("format tracking CSV"); + } + + let direct_trials = planning.iter().filter(|row| row.direct_free).count(); + let direct_successes = planning + .iter() + .filter(|row| row.direct_free && row.status == PlanStatus::Success) + .count(); + let obstructed_trials = planning.iter().filter(|row| !row.direct_free).count(); + let obstructed_successes = planning + .iter() + .filter(|row| !row.direct_free && row.status == PlanStatus::Success) + .count(); + let mut report = format!( + "# UR5e multi-scene, multi-query benchmark (simulation)\n\n\ + This deterministic extension evaluates **nine fixed scene-query fixtures (three per scene, six unique joint-pair definitions) across {} shipped MJCF scenes**. Repeating selected joint pairs across scenes creates controlled geometry comparisons. Five fixed planner seeds per fixture produce {} planning trials. The canonical-seed trajectory for each fixture is then replayed using two controller variants against the nominal plant and one fixed combined shift, producing {} tracking trials. Three fixtures have collision-blocked straight interpolants. **This is simulation evidence, not hardware validation or a sim-to-real guarantee.**\n\n\ + Planner headline: **{direct_successes}/{direct_trials} direct-free trials** and **{obstructed_successes}/{obstructed_trials} obstructed trials** succeeded.\n\n\ + The numeric tracking limits are reused unchanged from the earlier robustness envelope: temporal RMS six-joint L2 error <= {:.2} rad, maximum error <= {:.2} rad, and final error after a {}-step hold <= {:.2} rad. In addition, a tracking case passes only with **zero sampled robot-penetration steps** across settling, path execution, and hold. The audit uses a contact threshold of exactly 0.0 m and counts only MuJoCo-emitted robot contacts with signed distance `< 0`; signed distance, maximum actual penetration, and worst geom/body pair are retained in the raw CSV. This is not a positive-clearance certificate.\n\n\ + `plan_elapsed_ms` is observational wall-clock data: it is machine- and load-dependent and is **not byte-stable**. The bounded `--check` mode ignores only that column while verifying every committed deterministic planning field, all tracking fields/outcomes, and this report.\n\n\ + ## Planning and trajectory summary\n\n\ + | Scene | Query | Direct interpolant | Planner success | Cost range (rad) | Canonical trajectory |\n\ + |---|---|:---:|:---:|---:|---:|\n", + SCENES.len(), + planning.len(), + tracking.len(), + PASS_RMS_RAD, + PASS_MAX_RAD, + HOLD_STEPS, + PASS_FINAL_RAD + ); + + for query in QUERIES { + let scene = SCENES[query.scene]; + let rows: Vec<&PlanningRow> = planning + .iter() + .filter(|row| row.scene == scene.name && row.query == query.name) + .collect(); + let successes = rows + .iter() + .filter(|row| row.status == PlanStatus::Success) + .count(); + let min_cost = rows + .iter() + .map(|row| row.path_cost_rad) + .fold(f64::INFINITY, f64::min); + let max_cost = rows + .iter() + .map(|row| row.path_cost_rad) + .fold(0.0f64, f64::max); + let canonical = rows + .iter() + .find(|row| row.seed == CANONICAL_SEED) + .expect("canonical planning row"); + writeln!( + report, + "| {} | {} | {} | {}/{} | {:.3}--{:.3} | {} samples / {:.2} s |", + scene.name, + query.name, + if canonical.direct_free { + "free" + } else { + "blocked" + }, + successes, + rows.len(), + min_cost, + max_cost, + canonical.trajectory_samples, + canonical.trajectory_duration_s + ) + .expect("format planning table"); + } + + report.push_str( + "\n## Tracking results\n\n\ + Each cell is RMS / maximum / final six-joint L2 error in radians, followed by sampled penetration steps in settle/path/hold and maximum actual penetration. `PASS` requires all three numeric limits and zero penetration steps.\n\n\ + | Scene | Query | Plant | Position PD | PD + velocity FF |\n\ + |---|---|---|---:|---:|\n", + ); + for query in QUERIES { + let scene = SCENES[query.scene]; + for plant in PLANTS { + let position = tracking + .iter() + .find(|row| { + row.scene == scene.name + && row.query == query.name + && row.plant == plant.name + && row.controller == Controller::Position.name() + }) + .expect("position row"); + let velocity = tracking + .iter() + .find(|row| { + row.scene == scene.name + && row.query == query.name + && row.plant == plant.name + && row.controller == Controller::VelocityFf.name() + }) + .expect("velocity row"); + writeln!( + report, + "| {} | {} | {} | {:.4}/{:.4}/{:.4}; p {}/{}/{}; pen {:.3} mm {} | {:.4}/{:.4}/{:.4}; p {}/{}/{}; pen {:.3} mm {} |", + scene.name, + query.name, + plant.name, + position.rms_joint_rad, + position.max_joint_rad, + position.final_joint_rad, + position.settle_collisions.steps, + position.path_collisions.steps, + position.hold_collisions.steps, + 1e3 * position.max_penetration_m(), + if position.pass { "PASS" } else { "FAIL" }, + velocity.rms_joint_rad, + velocity.max_joint_rad, + velocity.final_joint_rad, + velocity.settle_collisions.steps, + velocity.path_collisions.steps, + velocity.hold_collisions.steps, + 1e3 * velocity.max_penetration_m(), + if velocity.pass { "PASS" } else { "FAIL" } + ) + .expect("format tracking table"); + } + } + + let position_passes = tracking + .iter() + .filter(|row| row.controller == Controller::Position.name() && row.pass) + .count(); + let velocity_passes = tracking + .iter() + .filter(|row| row.controller == Controller::VelocityFf.name() && row.pass) + .count(); + let position_numeric_passes = tracking + .iter() + .filter(|row| row.controller == Controller::Position.name() && numeric_gates_pass(row)) + .count(); + let velocity_numeric_passes = tracking + .iter() + .filter(|row| row.controller == Controller::VelocityFf.name() && numeric_gates_pass(row)) + .count(); + let penetration_cases: Vec<&TrackingRow> = tracking + .iter() + .filter(|row| row.penetration_steps() > 0) + .collect(); + let total_penetration_steps: usize = penetration_cases + .iter() + .map(|row| row.penetration_steps()) + .sum(); + let max_penetration_m = penetration_cases + .iter() + .map(|row| row.max_penetration_m()) + .fold(0.0f64, f64::max); + writeln!( + report, + "\n## Aggregate result\n\n- Direct-free planner trials: {direct_successes}/{direct_trials} succeeded.\n- Obstructed planner trials: {obstructed_successes}/{obstructed_trials} succeeded.\n- Position PD: {position_numeric_passes}/{} meet the numeric tracking gates; {position_passes}/{} pass after the zero-penetration requirement.\n- PD + velocity feedforward: {velocity_numeric_passes}/{} meet the numeric tracking gates; {velocity_passes}/{} pass after the zero-penetration requirement.\n- Executed-penetration audit: {}/{} cases have sampled robot penetration; {total_penetration_steps} total phase-steps, maximum actual penetration {:.8} m.\n", + tracking.len() / 2, + tracking.len() / 2, + tracking.len() / 2, + tracking.len() / 2, + penetration_cases.len(), + tracking.len(), + max_penetration_m + ) + .expect("format aggregate result"); + + if !penetration_cases.is_empty() { + report.push_str( + "\n## Executed penetration cases\n\n\ + Penetration steps are shown as settle/path/hold. The worst emitted contact is selected by maximum actual penetration; every listed signed distance is below zero.\n\n\ + | Scene | Query | Plant | Controller | Steps S/P/H | Max penetration (m) | Worst phase / signed distance / geom pair |\n\ + |---|---|---|---|---:|---:|---|\n", + ); + for row in &penetration_cases { + let (phase, worst) = worst_collision(row); + writeln!( + report, + "| {} | {} | {} | {} | {}/{}/{} | {:.8} | {} / {:.8} / {} |", + row.scene, + row.query, + row.plant, + row.controller, + row.settle_collisions.steps, + row.path_collisions.steps, + row.hold_collisions.steps, + row.max_penetration_m(), + phase, + worst + .worst_contact_distance_m + .expect("colliding phase signed distance"), + worst + .worst_contact + .as_deref() + .expect("colliding phase identity") + ) + .expect("format collision case"); + } + } + + writeln!( + report, + "\nThe fixed `tabletop_pillar/reverse_cross_workspace` fixture is retained in full, including any penetration-failing outcomes; no fixture or failed case is removed from either artifact." + ) + .expect("format retained negative statement"); + + report.push_str( + "\n## Exact scope and limitations\n\n\ + - The fixtures are deterministic and hand-designed, not sampled from a scene or query distribution. These counts are not estimates of a workspace-wide success probability.\n\ + - All scenes use the same UR5e model and actuator interface. The open scene contains only a floor; the other two scenes are distinct layouts but both use pillar-like obstacles.\n\ + - Five planner seeds probe sampling variability, but controller tracking uses one canonical path per query. `plan_elapsed_ms` is machine- and load-dependent, is not byte-stable, and is the only field ignored by `--check`.\n\ + - Only position PD and its desired-velocity-feedforward variant are compared here. This extension does not show that nominal-bias or integral-residual results generalize across queries.\n\ + - The combined plant is one deterministic condition: 1 kg payload at 0.10 m, 80% actuator gains, +1 Nms/rad joint damping, 10 ms command latency, and a 10 Nm / 120 ms shoulder-lift pulse at 45% of each trajectory. It is not a randomized uncertainty distribution.\n\ + - Planner collision checks remain discrete at 0.05 rad in joint-space L2 and use MuJoCo's emitted-contact set. The repository's positive contact threshold filters emitted candidates; with these zero-margin geoms it does not establish positive geometric clearance. Planner behavior is retained, but no 1-mm-clearance claim is made.\n\ + - The execution gate checks signed distance `< 0` at each 2-ms simulation state in settle, path, and hold. It detects sampled penetration, not positive-distance near misses or continuous swept-volume collision between samples. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk.\n\ + - There is no sensor noise, contact-rich grasping, hardware experiment, or sim-to-real guarantee.\n\n\ + ## Reproduce\n\n\ + ```bash\n\ + cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write\n\ + cargo run --release -p arm-lab-demo --bin multi_query_bench -- --check\n\ + ```\n\n\ + Raw artifacts: `docs/multi_query_planning.csv` (all 45 planner trials, including exact joint vectors) and `docs/multi_query_tracking.csv` (all 36 tracking trials, numeric metrics, per-phase penetration counts/depths, and worst emitted-contact identities).\n", + ); + + Artifacts { + planning_csv, + tracking_csv, + report, + } +} + +fn docs_dir() -> std::path::PathBuf { + Path::new(env!("CARGO_MANIFEST_DIR")).join("../../docs") +} + +fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { + let docs = docs_dir(); + std::fs::create_dir_all(&docs).expect("create docs directory"); + let artifacts = render_artifacts(planning, tracking); + std::fs::write( + docs.join("multi_query_planning.csv"), + artifacts.planning_csv, + ) + .expect("write planning CSV"); + std::fs::write( + docs.join("multi_query_tracking.csv"), + artifacts.tracking_csv, + ) + .expect("write tracking CSV"); + std::fs::write(docs.join("multi_query_results.md"), artifacts.report) + .expect("write Markdown report"); + println!( + "wrote docs/multi_query_planning.csv, docs/multi_query_tracking.csv, and docs/multi_query_results.md" + ); +} + +fn check_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { + let docs = docs_dir(); + let generated = render_artifacts(planning, tracking); + let committed_planning = std::fs::read_to_string(docs.join("multi_query_planning.csv")) + .expect("read committed planning CSV"); + let committed_tracking = std::fs::read_to_string(docs.join("multi_query_tracking.csv")) + .expect("read committed tracking CSV"); + let committed_report = std::fs::read_to_string(docs.join("multi_query_results.md")) + .expect("read committed Markdown report"); + + assert_artifact_equal( + "planning CSV deterministic fields", + &normalize_planning_elapsed(&generated.planning_csv), + &normalize_planning_elapsed(&committed_planning), + ); + assert_artifact_equal("tracking CSV", &generated.tracking_csv, &committed_tracking); + assert_artifact_equal("Markdown report", &generated.report, &committed_report); + println!( + "artifact check passed: all deterministic fields and outcomes match; plan_elapsed_ms ignored" + ); +} + +fn normalize_planning_elapsed(csv: &str) -> String { + let mut lines = csv.lines(); + let header = lines.next().expect("planning CSV header"); + let columns: Vec<&str> = header.split(',').collect(); + let elapsed_index = columns + .iter() + .position(|column| *column == "plan_elapsed_ms") + .expect("plan_elapsed_ms column"); + let mut normalized = format!("{header}\n"); + for line in lines { + let mut fields: Vec<&str> = line.split(',').collect(); + assert_eq!( + fields.len(), + columns.len(), + "planning CSV row has wrong column count" + ); + fields[elapsed_index] = ""; + writeln!(normalized, "{}", fields.join(",")).expect("normalize planning CSV"); + } + normalized +} + +fn assert_artifact_equal(label: &str, generated: &str, committed: &str) { + if generated == committed { + return; + } + let mismatch = generated + .lines() + .zip(committed.lines()) + .position(|(left, right)| left != right) + .map_or_else( + || generated.lines().count().min(committed.lines().count()) + 1, + |index| index + 1, + ); + panic!( + "stale {label}: first mismatch at line {mismatch}; rerun with --write and audit changes" + ); +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn benchmark_layout_is_three_scenes_by_three_queries() { + assert_eq!(SCENES.len(), 3); + assert_eq!(QUERIES.len(), 9); + for index in 0..SCENES.len() { + assert_eq!( + QUERIES.iter().filter(|query| query.scene == index).count(), + 3 + ); + } + } + + #[test] + fn canonical_seed_is_in_planner_seed_set() { + assert!(SEEDS.contains(&CANONICAL_SEED)); + } + + #[test] + fn repeated_fixtures_leave_six_unique_joint_pairs() { + let mut unique = Vec::new(); + for query in QUERIES { + let pair = (query.start_delta, query.goal_delta); + if !unique.contains(&pair) { + unique.push(pair); + } + } + assert_eq!(unique.len(), 6); + } + + #[test] + fn artifact_normalizer_ignores_only_wall_clock_column() { + let header = "scene,plan_elapsed_ms,status\n"; + let first = format!("{header}open,1.25000000,success\n"); + let timing_only = format!("{header}open,99.00000000,success\n"); + let stale_status = format!("{header}open,1.25000000,unconnected\n"); + assert_eq!( + normalize_planning_elapsed(&first), + normalize_planning_elapsed(&timing_only) + ); + assert_ne!( + normalize_planning_elapsed(&first), + normalize_planning_elapsed(&stale_status) + ); + } + + #[test] + fn execution_audit_uses_zero_threshold_and_actual_penetration() { + let signed_distance: f64 = -0.004; + let penetration = (-signed_distance).max(0.0); + assert_eq!(EXECUTION_CONTACT_THRESHOLD_M, 0.0); + assert!((penetration - 0.004).abs() < 1e-12); + } + + #[test] + fn penetration_step_invalidates_otherwise_passing_case() { + assert!(case_pass(0.01, 0.02, 0.01, 0)); + assert!(!case_pass(0.01, 0.02, 0.01, 1)); + } + + #[test] + #[should_panic(expected = "stale fixture")] + fn artifact_comparison_rejects_stale_deterministic_content() { + assert_artifact_equal("fixture", "expected\n", "stale\n"); + } +} diff --git a/crates/arm-lab/src/collision.rs b/crates/arm-lab/src/collision.rs index 9095256..be6bc29 100644 --- a/crates/arm-lab/src/collision.rs +++ b/crates/arm-lab/src/collision.rs @@ -1,14 +1,21 @@ //! Collision queries against a compiled MuJoCo scene. //! //! The checker writes a joint configuration into `qpos`, runs MuJoCo -//! kinematics + `mj_collision`, and reports a hit if any contact that -//! involves a robot collision geom has signed distance below `clearance`. +//! kinematics + `mj_collision`, and reports a hit if any **MuJoCo-emitted** +//! contact that involves a robot collision geom has signed distance below +//! `contact_threshold`. //! //! Parent–child and same-body pairs are already excluded by MuJoCo, so //! adjacent-link "contacts" never appear. Visual meshes (`contype = 0`) //! are ignored. Contacts that do not involve the robot (floor-vs-obstacle) //! are ignored too — those would otherwise flag every scene that has a //! pillar sitting on the ground plane. +//! +//! Important: `contact_threshold` filters contacts already emitted by MuJoCo; +//! it is not a general pairwise-distance query. With zero geom margin/gap, +//! separated positive-distance pairs might not be emitted even when their +//! distance is below a positive threshold. Use a threshold of `0.0` when the +//! required claim is sampled geometric penetration (`dist < 0`). use std::ops::Deref; @@ -16,6 +23,37 @@ use mujoco_rs::prelude::*; use crate::chain::Chain; +/// A robot-involved emitted MuJoCo contact below the checker's threshold. +/// +/// `distance_m` is MuJoCo's signed contact distance: negative values are +/// penetration. A positive value only means this pair happened to be emitted; +/// it does not prove that every closer positive-distance pair was enumerated. +#[derive(Debug, Clone, PartialEq)] +pub struct RobotContact { + pub geom1_id: i32, + pub geom2_id: i32, + pub geom1_name: String, + pub geom2_name: String, + pub body1_name: String, + pub body2_name: String, + pub distance_m: f64, +} + +impl RobotContact { + /// Stable human-readable identity for raw benchmark artifacts. + pub fn identity(&self) -> String { + format!( + "{}[{}]@{} vs {}[{}]@{}", + self.geom1_name, + self.geom1_id, + self.body1_name, + self.geom2_name, + self.geom2_id, + self.body2_name + ) + } +} + /// MuJoCo-backed collision oracle for a serial chain. pub struct CollisionChecker> { data: MjData, @@ -23,9 +61,12 @@ pub struct CollisionChecker> { /// `true` for geoms that belong to the robot and participate in contact /// (`contype != 0`). Indexed by MuJoCo geom id. robot_geom: Vec, - /// A contact counts as a collision when `dist < clearance` (meters). - /// Positive clearance inflates obstacles by that amount. - pub clearance: f64, + /// An **emitted** contact counts as a collision when + /// `dist < contact_threshold` (meters). + /// + /// This is a filter, not geometric inflation. Positive-distance pairs are + /// only considered when MuJoCo emitted them based on model margin/gap. + pub contact_threshold: f64, } impl> CollisionChecker { @@ -58,7 +99,7 @@ impl> CollisionChecker { data, qpos_adr, robot_geom, - clearance: 1e-3, + contact_threshold: 1e-3, } } @@ -77,22 +118,47 @@ impl> CollisionChecker { /// True if configuration `q` is in collision. pub fn collides(&mut self, q: &[f64]) -> bool { + self.update_contacts(q); + self.data.contact().iter().any(|contact| { + contact.dist < self.contact_threshold && self.contact_involves_robot(contact.geom) + }) + } + + /// Robot-involved emitted contacts below [`Self::contact_threshold`]. + /// + /// This uses exactly the same contact filter and strict distance comparison + /// as [`Self::collides`], but retains geom identity and signed distance for + /// execution audits. + pub fn robot_contacts(&mut self, q: &[f64]) -> Vec { + self.update_contacts(q); + let model = self.data.model(); + self.data + .contact() + .iter() + .filter(|contact| { + contact.dist < self.contact_threshold && self.contact_involves_robot(contact.geom) + }) + .map(|contact| RobotContact { + geom1_id: contact.geom[0], + geom2_id: contact.geom[1], + geom1_name: geom_name(model, contact.geom[0]), + geom2_name: geom_name(model, contact.geom[1]), + body1_name: geom_body_name(model, contact.geom[0]), + body2_name: geom_body_name(model, contact.geom[1]), + distance_m: contact.dist, + }) + .collect() + } + + fn update_contacts(&mut self, q: &[f64]) { self.set_q(q); self.data.forward_kinematics(); self.data.collision(); - for c in self.data.contact() { - if c.dist >= self.clearance { - continue; - } - let g1 = c.geom[0]; - let g2 = c.geom[1]; - let r1 = g1 >= 0 && self.robot_geom[g1 as usize]; - let r2 = g2 >= 0 && self.robot_geom[g2 as usize]; - if r1 || r2 { - return true; - } - } - false + } + + fn contact_involves_robot(&self, geom: [i32; 2]) -> bool { + geom.into_iter() + .any(|id| id >= 0 && self.robot_geom[id as usize]) } /// Borrow the inner [`MjData`] (e.g. to read body poses after `set_q`). @@ -105,3 +171,24 @@ impl> CollisionChecker { &mut self.data } } + +fn geom_name(model: &MjModel, id: i32) -> String { + if id < 0 { + return "none".to_string(); + } + model + .id_to_name(MjtObj::mjOBJ_GEOM, id as usize) + .filter(|name| !name.is_empty()) + .map_or_else(|| format!("geom#{id}"), str::to_string) +} + +fn geom_body_name(model: &MjModel, geom_id: i32) -> String { + if geom_id < 0 { + return "none".to_string(); + } + let body_id = model.geom_bodyid()[geom_id as usize] as usize; + model + .id_to_name(MjtObj::mjOBJ_BODY, body_id) + .filter(|name| !name.is_empty()) + .map_or_else(|| format!("body#{body_id}"), str::to_string) +} diff --git a/crates/arm-lab/src/lib.rs b/crates/arm-lab/src/lib.rs index c831b66..562798f 100644 --- a/crates/arm-lab/src/lib.rs +++ b/crates/arm-lab/src/lib.rs @@ -29,7 +29,7 @@ pub mod rng; pub mod traj; pub use chain::{Chain, Joint, Link}; -pub use collision::CollisionChecker; +pub use collision::{CollisionChecker, RobotContact}; pub use ik::{IkConfig, IkResult}; pub use plan::{PlanConfig, PlanResult, PlanStatus}; pub use rng::Rng; diff --git a/crates/arm-lab/tests/pickplace.rs b/crates/arm-lab/tests/pickplace.rs index 9c595f3..7815117 100644 --- a/crates/arm-lab/tests/pickplace.rs +++ b/crates/arm-lab/tests/pickplace.rs @@ -18,7 +18,8 @@ const PLACE_APPROACH: [f64; 3] = [0.22, 0.58, 0.52]; const PICK: [f64; 3] = [-0.24, 0.58, 0.42]; const PLACE: [f64; 3] = [0.22, 0.58, 0.42]; const SEED: u64 = 20260816; -const CARRY_CLEARANCE: f64 = 0.04; +// The cube is absent from planner geometry; this only rejects robot penetration. +const CARRY_CONTACT_THRESHOLD: f64 = 0.0; fn load() -> (MjModel, Chain, Vec) { let model = MjModel::from_xml(SCENE).unwrap(); @@ -70,11 +71,19 @@ fn home_is_free_and_poses_are_reachable() { } } +#[test] +fn cube_is_visual_only() { + let (model, _, _) = load(); + let cube_geom = model.name_to_id(MjtObj::mjOBJ_GEOM, "cube").unwrap(); + assert_eq!(model.geom_contype()[cube_geom], 0); + assert_eq!(model.geom_conaffinity()[cube_geom], 0); +} + #[test] fn carry_straight_line_hits_pillar_rrt_succeeds() { let (model, chain, q_home) = load(); let mut cc = CollisionChecker::new(&model, &chain); - cc.clearance = CARRY_CLEARANCE; + cc.contact_threshold = CARRY_CONTACT_THRESHOLD; let rot = fk(&chain, &q_home).rotation; let q_pick = ik_at(&chain, PICK_APPROACH, &q_home, rot); let q_place = ik_at(&chain, PLACE_APPROACH, &q_home, rot); diff --git a/crates/arm-lab/tests/plan.rs b/crates/arm-lab/tests/plan.rs index 5bf96c8..33a7768 100644 --- a/crates/arm-lab/tests/plan.rs +++ b/crates/arm-lab/tests/plan.rs @@ -175,6 +175,39 @@ fn pillar_is_detected() { ); } +#[test] +fn collision_audit_matches_boolean_and_identifies_pillar() { + let model = cluttered_model(); + let chain = cluttered_chain(&model); + let start = home_q(); + let mut q = start; + let mut checker = CollisionChecker::new(&model, &chain); + checker.contact_threshold = 0.0; + let mut first_contacts = None; + for step in 0..=100 { + q[0] = start[0] + 1.1 * step as f64 / 100.0; + let contacts = checker.robot_contacts(&q); + if !contacts.is_empty() { + assert!(checker.collides(&q)); + first_contacts = Some(contacts); + break; + } + } + let contacts = first_contacts.expect("pan sweep never contacted pillar"); + assert!(contacts.iter().all(|contact| { + contact.distance_m < 0.0 + && !contact.geom1_name.is_empty() + && !contact.geom2_name.is_empty() + && !contact.body1_name.is_empty() + && !contact.body2_name.is_empty() + })); + assert!( + contacts + .iter() + .any(|contact| { contact.geom1_name == "pillar" || contact.geom2_name == "pillar" }) + ); +} + #[test] fn rrt_dodges_ur5e_pillar() { let model = cluttered_model(); diff --git a/docs/multi_query_planning.csv b/docs/multi_query_planning.csv new file mode 100644 index 0000000..09815cf --- /dev/null +++ b/docs/multi_query_planning.csv @@ -0,0 +1,46 @@ +scene,scene_file,query,seed,start_q_rad,goal_q_rad,direct_path_free,status,plan_elapsed_ms,iterations,nodes,shortcut_waypoints,path_samples,path_cost_rad,trajectory_samples,trajectory_duration_s +open_floor,scene.xml,positive_pan,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.12823000,0,2,2,23,1.10000000,1267,2.53055556 +open_floor,scene.xml,positive_pan,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.14538000,0,2,2,23,1.10000000,1267,2.53055556 +open_floor,scene.xml,positive_pan,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.15742000,0,2,2,23,1.10000000,1267,2.53055556 +open_floor,scene.xml,positive_pan,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.12721900,0,2,2,23,1.10000000,1267,2.53055556 +open_floor,scene.xml,positive_pan,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.14511000,0,2,2,23,1.10000000,1267,2.53055556 +open_floor,scene.xml,shoulder_elbow,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.09794000,0,2,2,18,0.80467385,767,1.53055556 +open_floor,scene.xml,shoulder_elbow,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.10656000,0,2,2,18,0.80467385,767,1.53055556 +open_floor,scene.xml,shoulder_elbow,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.09560000,0,2,2,18,0.80467385,767,1.53055556 +open_floor,scene.xml,shoulder_elbow,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.09538000,0,2,2,18,0.80467385,767,1.53055556 +open_floor,scene.xml,shoulder_elbow,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.10071000,0,2,2,18,0.80467385,767,1.53055556 +open_floor,scene.xml,wrist_reorientation,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.12502000,0,2,2,23,1.05830052,903,1.80328283 +open_floor,scene.xml,wrist_reorientation,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.11945000,0,2,2,23,1.05830052,903,1.80328283 +open_floor,scene.xml,wrist_reorientation,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.12562900,0,2,2,23,1.05830052,903,1.80328283 +open_floor,scene.xml,wrist_reorientation,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.12008000,0,2,2,23,1.05830052,903,1.80328283 +open_floor,scene.xml,wrist_reorientation,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.12804000,0,2,2,23,1.05830052,903,1.80328283 +offset_pillar,scene_cluttered.xml,positive_pan,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,false,success,6.40932000,69,64,3,53,2.54129475,1751,3.49937200 +offset_pillar,scene_cluttered.xml,positive_pan,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,false,success,10.36771400,71,69,4,45,2.17517403,1741,3.47972176 +offset_pillar,scene_cluttered.xml,positive_pan,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,false,success,6.77086900,53,44,3,40,1.86608655,1324,2.64500127 +offset_pillar,scene_cluttered.xml,positive_pan,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,false,success,7.61776800,121,87,3,38,1.80315752,1301,2.59962619 +offset_pillar,scene_cluttered.xml,positive_pan,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-0.47080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,false,success,6.91756900,85,74,3,42,1.95497101,1320,2.63606050 +offset_pillar,scene_cluttered.xml,negative_pan,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-2.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.12143000,0,2,2,22,1.00000000,1176,2.34873737 +offset_pillar,scene_cluttered.xml,negative_pan,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-2.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.12102000,0,2,2,22,1.00000000,1176,2.34873737 +offset_pillar,scene_cluttered.xml,negative_pan,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-2.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.12090000,0,2,2,22,1.00000000,1176,2.34873737 +offset_pillar,scene_cluttered.xml,negative_pan,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-2.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.11982000,0,2,2,22,1.00000000,1176,2.34873737 +offset_pillar,scene_cluttered.xml,negative_pan,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-2.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,true,success,0.11953000,0,2,2,22,1.00000000,1176,2.34873737 +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.10970000,0,2,2,18,0.80467385,767,1.53055556 +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.11961000,0,2,2,18,0.80467385,767,1.53055556 +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.10472900,0,2,2,18,0.80467385,767,1.53055556 +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.10456000,0,2,2,18,0.80467385,767,1.53055556 +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.22080000;-1.17080000;1.02080000;-1.32080000;-1.57080000;0.00000000,true,success,0.10455000,0,2,2,18,0.80467385,767,1.53055556 +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260816,-1.97080000;-1.42080000;1.37080000;-1.47080000;-1.57080000;0.00000000,-0.82080000;-1.32080000;1.12080000;-1.32080000;-1.42080000;-0.20000000,false,success,1.42804800,1,9,3,32,1.51811648,1515,3.02746517 +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260817,-1.97080000;-1.42080000;1.37080000;-1.47080000;-1.57080000;0.00000000,-0.82080000;-1.32080000;1.12080000;-1.32080000;-1.42080000;-0.20000000,false,success,2.35383600,14,21,3,35,1.60322265,1651,3.29821051 +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260818,-1.97080000;-1.42080000;1.37080000;-1.47080000;-1.57080000;0.00000000,-0.82080000;-1.32080000;1.12080000;-1.32080000;-1.42080000;-0.20000000,false,success,9.71415400,219,124,4,35,1.66097912,1634,3.26532736 +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260819,-1.97080000;-1.42080000;1.37080000;-1.47080000;-1.57080000;0.00000000,-0.82080000;-1.32080000;1.12080000;-1.32080000;-1.42080000;-0.20000000,false,success,6.25459000,103,69,4,46,2.20622753,1972,3.94023314 +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260820,-1.97080000;-1.42080000;1.37080000;-1.47080000;-1.57080000;0.00000000,-0.82080000;-1.32080000;1.12080000;-1.32080000;-1.42080000;-0.20000000,false,success,4.31853300,75,58,4,40,1.93369592,1925,3.84732284 +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260816,-0.92080000;-1.37080000;1.22080000;-1.42080000;-1.47080000;-0.20000000,-2.02080000;-1.47080000;1.47080000;-1.67080000;-1.77080000;0.35000000,false,success,5.12020200,21,25,4,36,1.71473352,1679,3.35412207 +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260817,-0.92080000;-1.37080000;1.22080000;-1.42080000;-1.47080000;-0.20000000,-2.02080000;-1.47080000;1.47080000;-1.67080000;-1.77080000;0.35000000,false,success,7.12940900,9,16,3,36,1.67520170,1506,3.00864782 +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260818,-0.92080000;-1.37080000;1.22080000;-1.42080000;-1.47080000;-0.20000000,-2.02080000;-1.47080000;1.47080000;-1.67080000;-1.77080000;0.35000000,false,success,5.28172200,2,9,3,32,1.47459528,1296,2.58979659 +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260819,-0.92080000;-1.37080000;1.22080000;-1.42080000;-1.47080000;-0.20000000,-2.02080000;-1.47080000;1.47080000;-1.67080000;-1.77080000;0.35000000,false,success,5.80245100,10,16,3,32,1.47536270,1358,2.71385271 +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260820,-0.92080000;-1.37080000;1.22080000;-1.42080000;-1.47080000;-0.20000000,-2.02080000;-1.47080000;1.47080000;-1.67080000;-1.77080000;0.35000000,false,success,6.44594900,24,26,3,34,1.57295370,1502,3.00193184 +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260816,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.21792000,0,2,2,23,1.05830052,903,1.80328283 +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260817,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.21204000,0,2,2,23,1.05830052,903,1.80328283 +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260818,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.21623000,0,2,2,23,1.05830052,903,1.80328283 +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260819,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.20825900,0,2,2,23,1.05830052,903,1.80328283 +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260820,-1.57080000;-1.57080000;1.57080000;-1.57080000;-1.57080000;0.00000000,-1.32080000;-1.72080000;1.82080000;-1.92080000;-0.97080000;-0.70000000,true,success,0.21014000,0,2,2,23,1.05830052,903,1.80328283 diff --git a/docs/multi_query_results.md b/docs/multi_query_results.md new file mode 100644 index 0000000..2d1e0bd --- /dev/null +++ b/docs/multi_query_results.md @@ -0,0 +1,90 @@ +# UR5e multi-scene, multi-query benchmark (simulation) + +This deterministic extension evaluates **nine fixed scene-query fixtures (three per scene, six unique joint-pair definitions) across 3 shipped MJCF scenes**. Repeating selected joint pairs across scenes creates controlled geometry comparisons. Five fixed planner seeds per fixture produce 45 planning trials. The canonical-seed trajectory for each fixture is then replayed using two controller variants against the nominal plant and one fixed combined shift, producing 36 tracking trials. Three fixtures have collision-blocked straight interpolants. **This is simulation evidence, not hardware validation or a sim-to-real guarantee.** + +Planner headline: **30/30 direct-free trials** and **15/15 obstructed trials** succeeded. + +The numeric tracking limits are reused unchanged from the earlier robustness envelope: temporal RMS six-joint L2 error <= 0.03 rad, maximum error <= 0.10 rad, and final error after a 250-step hold <= 0.02 rad. In addition, a tracking case passes only with **zero sampled robot-penetration steps** across settling, path execution, and hold. The audit uses a contact threshold of exactly 0.0 m and counts only MuJoCo-emitted robot contacts with signed distance `< 0`; signed distance, maximum actual penetration, and worst geom/body pair are retained in the raw CSV. This is not a positive-clearance certificate. + +`plan_elapsed_ms` is observational wall-clock data: it is machine- and load-dependent and is **not byte-stable**. The bounded `--check` mode ignores only that column while verifying every committed deterministic planning field, all tracking fields/outcomes, and this report. + +## Planning and trajectory summary + +| Scene | Query | Direct interpolant | Planner success | Cost range (rad) | Canonical trajectory | +|---|---|:---:|:---:|---:|---:| +| open_floor | positive_pan | free | 5/5 | 1.100--1.100 | 1267 samples / 2.53 s | +| open_floor | shoulder_elbow | free | 5/5 | 0.805--0.805 | 767 samples / 1.53 s | +| open_floor | wrist_reorientation | free | 5/5 | 1.058--1.058 | 903 samples / 1.80 s | +| offset_pillar | positive_pan | blocked | 5/5 | 1.803--2.541 | 1751 samples / 3.50 s | +| offset_pillar | negative_pan | free | 5/5 | 1.000--1.000 | 1176 samples / 2.35 s | +| offset_pillar | shoulder_elbow | free | 5/5 | 0.805--0.805 | 767 samples / 1.53 s | +| tabletop_pillar | cross_workspace | blocked | 5/5 | 1.518--2.206 | 1515 samples / 3.03 s | +| tabletop_pillar | reverse_cross_workspace | blocked | 5/5 | 1.475--1.715 | 1679 samples / 3.35 s | +| tabletop_pillar | wrist_reorientation | free | 5/5 | 1.058--1.058 | 903 samples / 1.80 s | + +## Tracking results + +Each cell is RMS / maximum / final six-joint L2 error in radians, followed by sampled penetration steps in settle/path/hold and maximum actual penetration. `PASS` requires all three numeric limits and zero penetration steps. + +| Scene | Query | Plant | Position PD | PD + velocity FF | +|---|---|---|---:|---:| +| open_floor | positive_pan | nominal | 0.0913/0.1095/0.0119; p 0/0/0; pen 0.000 mm FAIL | 0.0117/0.0118/0.0116; p 0/0/0; pen 0.000 mm PASS | +| open_floor | positive_pan | combined moderate shift | 0.0973/0.1163/0.0195; p 0/0/0; pen 0.000 mm FAIL | 0.0195/0.0202/0.0193; p 0/0/0; pen 0.000 mm PASS | +| open_floor | shoulder_elbow | nominal | 0.1117/0.1552/0.0173; p 0/0/0; pen 0.000 mm FAIL | 0.0140/0.0176/0.0176; p 0/0/0; pen 0.000 mm PASS | +| open_floor | shoulder_elbow | combined moderate shift | 0.1188/0.1643/0.0266; p 0/0/0; pen 0.000 mm FAIL | 0.0222/0.0270/0.0271; p 0/0/0; pen 0.000 mm FAIL | +| open_floor | wrist_reorientation | nominal | 0.1245/0.1640/0.0105; p 0/0/0; pen 0.000 mm FAIL | 0.0109/0.0116/0.0098; p 0/0/0; pen 0.000 mm PASS | +| open_floor | wrist_reorientation | combined moderate shift | 0.1339/0.1760/0.0187; p 0/0/0; pen 0.000 mm FAIL | 0.0200/0.0215/0.0176; p 0/0/0; pen 0.000 mm PASS | +| offset_pillar | positive_pan | nominal | 0.1409/0.1719/0.0111; p 0/0/0; pen 0.000 mm FAIL | 0.0097/0.0121/0.0117; p 0/0/0; pen 0.000 mm PASS | +| offset_pillar | positive_pan | combined moderate shift | 0.1498/0.1852/0.0187; p 0/0/0; pen 0.000 mm FAIL | 0.0186/0.0241/0.0194; p 0/0/0; pen 0.000 mm PASS | +| offset_pillar | negative_pan | nominal | 0.0897/0.1095/0.0120; p 0/0/0; pen 0.000 mm FAIL | 0.0117/0.0119/0.0117; p 0/0/0; pen 0.000 mm PASS | +| offset_pillar | negative_pan | combined moderate shift | 0.0957/0.1163/0.0196; p 0/0/0; pen 0.000 mm FAIL | 0.0195/0.0200/0.0193; p 0/0/0; pen 0.000 mm PASS | +| offset_pillar | shoulder_elbow | nominal | 0.1117/0.1552/0.0173; p 0/0/0; pen 0.000 mm FAIL | 0.0140/0.0176/0.0176; p 0/0/0; pen 0.000 mm PASS | +| offset_pillar | shoulder_elbow | combined moderate shift | 0.1188/0.1643/0.0266; p 0/0/0; pen 0.000 mm FAIL | 0.0222/0.0270/0.0271; p 0/0/0; pen 0.000 mm FAIL | +| tabletop_pillar | cross_workspace | nominal | 0.0998/0.1204/0.0152; p 0/0/0; pen 0.000 mm FAIL | 0.0132/0.0151/0.0152; p 0/0/0; pen 0.000 mm PASS | +| tabletop_pillar | cross_workspace | combined moderate shift | 0.1067/0.1278/0.0239; p 0/0/0; pen 0.000 mm FAIL | 0.0218/0.0252/0.0240; p 0/0/0; pen 0.000 mm FAIL | +| tabletop_pillar | reverse_cross_workspace | nominal | 0.1033/0.1213/0.0134; p 0/35/0; pen 0.055 mm FAIL | 0.0129/0.0143/0.0131; p 0/25/0; pen 0.050 mm FAIL | +| tabletop_pillar | reverse_cross_workspace | combined moderate shift | 0.1108/0.1315/0.0221; p 0/72/0; pen 0.079 mm FAIL | 0.0213/0.0241/0.0216; p 0/60/0; pen 0.079 mm FAIL | +| tabletop_pillar | wrist_reorientation | nominal | 0.1245/0.1640/0.0105; p 0/0/0; pen 0.000 mm FAIL | 0.0109/0.0116/0.0098; p 0/0/0; pen 0.000 mm PASS | +| tabletop_pillar | wrist_reorientation | combined moderate shift | 0.1339/0.1760/0.0187; p 0/0/0; pen 0.000 mm FAIL | 0.0200/0.0215/0.0176; p 0/0/0; pen 0.000 mm PASS | + +## Aggregate result + +- Direct-free planner trials: 30/30 succeeded. +- Obstructed planner trials: 15/15 succeeded. +- Position PD: 0/18 meet the numeric tracking gates; 0/18 pass after the zero-penetration requirement. +- PD + velocity feedforward: 14/18 meet the numeric tracking gates; 13/18 pass after the zero-penetration requirement. +- Executed-penetration audit: 4/36 cases have sampled robot penetration; 192 total phase-steps, maximum actual penetration 0.00007925 m. + + +## Executed penetration cases + +Penetration steps are shown as settle/path/hold. The worst emitted contact is selected by maximum actual penetration; every listed signed distance is below zero. + +| Scene | Query | Plant | Controller | Steps S/P/H | Max penetration (m) | Worst phase / signed distance / geom pair | +|---|---|---|---|---:|---:|---| +| tabletop_pillar | reverse_cross_workspace | nominal | position PD | 0/35/0 | 0.00005480 | path / -0.00005480 / geom#27[27]@wrist_2_link vs pillar[31]@pillar | +| tabletop_pillar | reverse_cross_workspace | nominal | PD + velocity FF | 0/25/0 | 0.00005013 | path / -0.00005013 / geom#27[27]@wrist_2_link vs pillar[31]@pillar | +| tabletop_pillar | reverse_cross_workspace | combined moderate shift | position PD | 0/72/0 | 0.00007925 | path / -0.00007925 / geom#27[27]@wrist_2_link vs pillar[31]@pillar | +| tabletop_pillar | reverse_cross_workspace | combined moderate shift | PD + velocity FF | 0/60/0 | 0.00007922 | path / -0.00007922 / geom#27[27]@wrist_2_link vs pillar[31]@pillar | + +The fixed `tabletop_pillar/reverse_cross_workspace` fixture is retained in full, including any penetration-failing outcomes; no fixture or failed case is removed from either artifact. + +## Exact scope and limitations + +- The fixtures are deterministic and hand-designed, not sampled from a scene or query distribution. These counts are not estimates of a workspace-wide success probability. +- All scenes use the same UR5e model and actuator interface. The open scene contains only a floor; the other two scenes are distinct layouts but both use pillar-like obstacles. +- Five planner seeds probe sampling variability, but controller tracking uses one canonical path per query. `plan_elapsed_ms` is machine- and load-dependent, is not byte-stable, and is the only field ignored by `--check`. +- Only position PD and its desired-velocity-feedforward variant are compared here. This extension does not show that nominal-bias or integral-residual results generalize across queries. +- The combined plant is one deterministic condition: 1 kg payload at 0.10 m, 80% actuator gains, +1 Nms/rad joint damping, 10 ms command latency, and a 10 Nm / 120 ms shoulder-lift pulse at 45% of each trajectory. It is not a randomized uncertainty distribution. +- Planner collision checks remain discrete at 0.05 rad in joint-space L2 and use MuJoCo's emitted-contact set. The repository's positive contact threshold filters emitted candidates; with these zero-margin geoms it does not establish positive geometric clearance. Planner behavior is retained, but no 1-mm-clearance claim is made. +- The execution gate checks signed distance `< 0` at each 2-ms simulation state in settle, path, and hold. It detects sampled penetration, not positive-distance near misses or continuous swept-volume collision between samples. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk. +- There is no sensor noise, contact-rich grasping, hardware experiment, or sim-to-real guarantee. + +## Reproduce + +```bash +cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write +cargo run --release -p arm-lab-demo --bin multi_query_bench -- --check +``` + +Raw artifacts: `docs/multi_query_planning.csv` (all 45 planner trials, including exact joint vectors) and `docs/multi_query_tracking.csv` (all 36 tracking trials, numeric metrics, per-phase penetration counts/depths, and worst emitted-contact identities). diff --git a/docs/multi_query_tracking.csv b/docs/multi_query_tracking.csv new file mode 100644 index 0000000..138208b --- /dev/null +++ b/docs/multi_query_tracking.csv @@ -0,0 +1,37 @@ +scene,scene_file,query,seed,direct_path_free,plant,controller,trajectory_samples,trajectory_duration_s,rms_joint_rad,max_joint_rad,final_joint_rad,max_ee_pos_m,peak_force_fraction,saturated_step_fraction,settle_penetration_steps,settle_max_penetration_m,settle_worst_contact_distance_m,settle_worst_contact,path_penetration_steps,path_max_penetration_m,path_worst_contact_distance_m,path_worst_contact,hold_penetration_steps,hold_max_penetration_m,hold_worst_contact_distance_m,hold_worst_contact,penetration_steps,max_penetration_m,pass +open_floor,scene.xml,positive_pan,20260816,true,nominal,position PD,1267,2.53055556,0.09129176,0.10951059,0.01189736,0.05658881,0.11131157,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,positive_pan,20260816,true,nominal,PD + velocity FF,1267,2.53055556,0.01167638,0.01177042,0.01164128,0.00836165,0.11324538,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +open_floor,scene.xml,positive_pan,20260816,true,combined moderate shift,position PD,1267,2.53055556,0.09731765,0.11634764,0.01948129,0.06073798,0.14793866,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,positive_pan,20260816,true,combined moderate shift,PD + velocity FF,1267,2.53055556,0.01954746,0.02020349,0.01932490,0.01410921,0.14794182,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +open_floor,scene.xml,shoulder_elbow,20260816,true,nominal,position PD,767,1.53055556,0.11174946,0.15522691,0.01725200,0.04531244,0.20979290,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,shoulder_elbow,20260816,true,nominal,PD + velocity FF,767,1.53055556,0.01403839,0.01762035,0.01759917,0.01464391,0.22205561,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +open_floor,scene.xml,shoulder_elbow,20260816,true,combined moderate shift,position PD,767,1.53055556,0.11875634,0.16432404,0.02664251,0.04898549,0.25144962,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,shoulder_elbow,20260816,true,combined moderate shift,PD + velocity FF,767,1.53055556,0.02222473,0.02701895,0.02713649,0.02242778,0.26370723,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,wrist_reorientation,20260816,true,nominal,position PD,903,1.80328283,0.12450657,0.16395815,0.01053793,0.03189809,0.11149208,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,wrist_reorientation,20260816,true,nominal,PD + velocity FF,903,1.80328283,0.01090525,0.01161888,0.00979879,0.00825368,0.11398148,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +open_floor,scene.xml,wrist_reorientation,20260816,true,combined moderate shift,position PD,903,1.80328283,0.13385055,0.17604234,0.01867555,0.03555215,0.14698090,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +open_floor,scene.xml,wrist_reorientation,20260816,true,combined moderate shift,PD + velocity FF,903,1.80328283,0.01997269,0.02150639,0.01763538,0.01368163,0.14750717,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +offset_pillar,scene_cluttered.xml,positive_pan,20260816,false,nominal,position PD,1751,3.49937200,0.14092183,0.17193787,0.01108060,0.07768279,0.12082634,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +offset_pillar,scene_cluttered.xml,positive_pan,20260816,false,nominal,PD + velocity FF,1751,3.49937200,0.00968930,0.01211322,0.01167408,0.00858596,1.00000000,0.00114220,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +offset_pillar,scene_cluttered.xml,positive_pan,20260816,false,combined moderate shift,position PD,1751,3.49937200,0.14977587,0.18515253,0.01866941,0.08562599,0.15629775,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +offset_pillar,scene_cluttered.xml,positive_pan,20260816,false,combined moderate shift,PD + velocity FF,1751,3.49937200,0.01857049,0.02406949,0.01937812,0.01683355,1.00000000,0.00114220,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +offset_pillar,scene_cluttered.xml,negative_pan,20260816,true,nominal,position PD,1176,2.34873737,0.08972564,0.10948313,0.01196390,0.05578537,0.11159613,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +offset_pillar,scene_cluttered.xml,negative_pan,20260816,true,nominal,PD + velocity FF,1176,2.34873737,0.01165001,0.01190326,0.01165928,0.00848648,0.11318029,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +offset_pillar,scene_cluttered.xml,negative_pan,20260816,true,combined moderate shift,position PD,1176,2.34873737,0.09568082,0.11631281,0.01955737,0.05955611,0.14797399,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +offset_pillar,scene_cluttered.xml,negative_pan,20260816,true,combined moderate shift,PD + velocity FF,1176,2.34873737,0.01949234,0.01996580,0.01934500,0.01378922,0.14794534,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260816,true,nominal,position PD,767,1.53055556,0.11174946,0.15522691,0.01725200,0.04531244,0.20979290,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260816,true,nominal,PD + velocity FF,767,1.53055556,0.01403839,0.01762035,0.01759917,0.01464391,0.22205561,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260816,true,combined moderate shift,position PD,767,1.53055556,0.11875634,0.16432404,0.02664251,0.04898549,0.25144962,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +offset_pillar,scene_cluttered.xml,shoulder_elbow,20260816,true,combined moderate shift,PD + velocity FF,767,1.53055556,0.02222473,0.02701895,0.02713649,0.02242778,0.26370723,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260816,false,nominal,position PD,1515,3.02746517,0.09979727,0.12044330,0.01516361,0.06385576,0.16914397,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260816,false,nominal,PD + velocity FF,1515,3.02746517,0.01316320,0.01512919,0.01517729,0.01215343,1.00000000,0.00066007,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260816,false,combined moderate shift,position PD,1515,3.02746517,0.10668095,0.12775363,0.02389145,0.06863752,0.20825102,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +tabletop_pillar,scene_pickplace.xml,cross_workspace,20260816,false,combined moderate shift,PD + velocity FF,1515,3.02746517,0.02181035,0.02523840,0.02400096,0.01907289,1.00000000,0.00066007,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260816,false,nominal,position PD,1679,3.35412207,0.10326798,0.12134823,0.01340133,0.05654237,0.29351119,0.00000000,0,0.00000000,,,35,0.00005480,-0.00005480,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,,,35,0.00005480,false +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260816,false,nominal,PD + velocity FF,1679,3.35412207,0.01287595,0.01425906,0.01307311,0.01127117,1.00000000,0.00119119,0,0.00000000,,,25,0.00005013,-0.00005013,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,,,25,0.00005013,false +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260816,false,combined moderate shift,position PD,1679,3.35412207,0.11081774,0.13149558,0.02206862,0.06128885,0.31697492,0.00000000,0,0.00000000,,,72,0.00007925,-0.00007925,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,,,72,0.00007925,false +tabletop_pillar,scene_pickplace.xml,reverse_cross_workspace,20260816,false,combined moderate shift,PD + velocity FF,1679,3.35412207,0.02130657,0.02405470,0.02159796,0.01831127,1.00000000,0.00059559,0,0.00000000,,,60,0.00007922,-0.00007922,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,,,60,0.00007922,false +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260816,true,nominal,position PD,903,1.80328283,0.12450657,0.16395815,0.01053793,0.03189809,0.11149208,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260816,true,nominal,PD + velocity FF,903,1.80328283,0.01090525,0.01161888,0.00979879,0.00825368,0.11398148,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260816,true,combined moderate shift,position PD,903,1.80328283,0.13385055,0.17604234,0.01867555,0.03555215,0.14698090,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,false +tabletop_pillar,scene_pickplace.xml,wrist_reorientation,20260816,true,combined moderate shift,PD + velocity FF,903,1.80328283,0.01997269,0.02150639,0.01763538,0.01368163,0.14750717,0.00000000,0,0.00000000,,,0,0.00000000,,,0,0.00000000,,,0,0.00000000,true