From 89d0bc6d2d9adb435f62741ba19d100dedf07a1d Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 01:49:37 +0100 Subject: [PATCH 1/7] Predeclare multi-scene benchmark matrix --- .../arm-lab-demo/src/bin/multi_query_bench.rs | 868 ++++++++++++++++++ 1 file changed, 868 insertions(+) create mode 100644 crates/arm-lab-demo/src/bin/multi_query_bench.rs 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..82deaf2 --- /dev/null +++ b/crates/arm-lab-demo/src/bin/multi_query_bench.rs @@ -0,0 +1,868 @@ +//! 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 queries across all three shipped UR5e scenes. The canonical-seed +//! trajectory for every query is then replayed with position PD and 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 +//! ``` + +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; +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, + pass: bool, +} + +struct DelayedCommand { + ctrl: Vec, +} + +fn main() { + let write_results = std::env::args().any(|arg| arg == "--write"); + 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} | {}", + metrics.plant, + metrics.controller, + metrics.rms_joint_rad, + metrics.max_joint_rad, + metrics.final_joint_rad, + 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 planner_successes = planning_rows + .iter() + .filter(|row| row.status == PlanStatus::Success) + .count(); + let velocity_passes = tracking_rows + .iter() + .filter(|row| row.controller == Controller::VelocityFf.name() && row.pass) + .count(); + println!( + "summary: {planner_successes}/{} planner successes; {blocked_queries}/{} blocked-direct queries; {velocity_passes}/{} velocity-FF passes", + planning_rows.len(), + QUERIES.len(), + QUERIES.len() * PLANTS.len() + ); + + if write_results { + write_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); + 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, + ); + } + + 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); + 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, + ); + } + let final_joint = l2(&read_q(&data, chain), q_goal); + let rms_joint = (sum_sq / samples as f64).sqrt(); + let pass = + rms_joint <= PASS_RMS_RAD && max_joint <= PASS_MAX_RAD && final_joint <= PASS_FINAL_RAD; + + 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, + 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 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(";") +} + +fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { + let docs = Path::new(env!("CARGO_MANIFEST_DIR")).join("../../docs"); + std::fs::create_dir_all(&docs).expect("create docs directory"); + + 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"); + } + std::fs::write(docs.join("multi_query_planning.csv"), planning_csv) + .expect("write 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,pass\n", + ); + for row in tracking { + writeln!( + tracking_csv, + "{},{},{},{},{},{},{},{},{:.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.pass + ) + .expect("format tracking CSV"); + } + std::fs::write(docs.join("multi_query_tracking.csv"), tracking_csv) + .expect("write tracking CSV"); + + let mut report = format!( + "# UR5e multi-scene, multi-query benchmark (simulation)\n\n\ + This deterministic extension evaluates **{} fixed queries across {} shipped MJCF scenes**. Five fixed planner seeds per query produce {} planning trials. The canonical-seed trajectory for each query is then replayed using two controller variants against the nominal plant and one fixed combined shift, producing {} tracking trials. Three queries have collision-blocked straight interpolants. **This is simulation evidence, not hardware validation or a sim-to-real guarantee.**\n\n\ + The tracking pass 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. They were declared in code before this benchmark was executed.\n\n\ + ## Planning and trajectory summary\n\n\ + | Scene | Query | Direct interpolant | Planner success | Median plan (ms) | Cost range (rad) | Canonical trajectory |\n\ + |---|---|:---:|:---:|---:|---:|---:|\n", + QUERIES.len(), + 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 mut rows: Vec<&PlanningRow> = planning + .iter() + .filter(|row| row.scene == scene.name && row.query == query.name) + .collect(); + rows.sort_by(|a, b| a.elapsed_ms.total_cmp(&b.elapsed_ms)); + 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}--{:.3} | {} samples / {:.2} s |", + scene.name, + query.name, + if canonical.direct_free { + "free" + } else { + "blocked" + }, + successes, + rows.len(), + rows[rows.len() / 2].elapsed_ms, + 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. `PASS` requires all three declared limits.\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} {} | {:.4} / {:.4} / {:.4} {} |", + scene.name, + query.name, + plant.name, + position.rms_joint_rad, + position.max_joint_rad, + position.final_joint_rad, + if position.pass { "PASS" } else { "FAIL" }, + velocity.rms_joint_rad, + velocity.max_joint_rad, + velocity.final_joint_rad, + if velocity.pass { "PASS" } else { "FAIL" } + ) + .expect("format tracking table"); + } + } + + let planner_passes = planning + .iter() + .filter(|row| row.status == PlanStatus::Success) + .count(); + 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(); + writeln!( + report, + "\n## Aggregate result\n\n- Planner: {planner_passes}/{} fixed-seed trials succeeded.\n- Position PD: {position_passes}/{} tracking cases passed.\n- PD + velocity feedforward: {velocity_passes}/{} tracking cases passed.\n", + planning.len(), + tracking.len() / 2, + tracking.len() / 2 + ) + .expect("format aggregate result"); + + 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. Wall-clock planning times are machine- and load-dependent.\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\ + - Collision checks remain discrete at 0.05 rad in joint-space L2. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk. There is no sensor noise, contact-rich grasping, or hardware experiment.\n\n\ + ## Reproduce\n\n\ + ```bash\n\ + cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write\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 and complete metrics).\n", + ); + + std::fs::write(docs.join("multi_query_results.md"), report).expect("write Markdown report"); + println!( + "wrote docs/multi_query_planning.csv, docs/multi_query_tracking.csv, and docs/multi_query_results.md" + ); +} + +#[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)); + } +} From 7d21ba3d127e06d6b810da5e36b1fed929ee13df Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 01:52:33 +0100 Subject: [PATCH 2/7] Add multi-scene planning and tracking evidence --- .github/workflows/ci.yml | 1 + README.md | 10 +++ .../arm-lab-demo/src/bin/multi_query_bench.rs | 53 ++++++++++++-- docs/multi_query_planning.csv | 46 +++++++++++++ docs/multi_query_results.md | 69 +++++++++++++++++++ docs/multi_query_tracking.csv | 37 ++++++++++ 6 files changed, 210 insertions(+), 6 deletions(-) create mode 100644 docs/multi_query_planning.csv create mode 100644 docs/multi_query_results.md create mode 100644 docs/multi_query_tracking.csv diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3170775..4c556de 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -38,6 +38,7 @@ 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 - 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..f48610f 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 tracking thresholds. Selected joint pairs are deliberately repeated across scenes to isolate geometry effects. Across five fixed planner seeds per fixture, all **45/45** plans succeeded; three fixtures had collision-blocked straight interpolants. Position PD passed **0/18** nominal-plus-combined-shift tracking cases. Adding desired-velocity feedforward passed **9/9 nominal** and **5/9 combined-shift** cases. The four combined-shift misses are retained in the raw evidence: their final errors exceed the 0.02-rad hold gate even though RMS and maximum errors remain inside their gates. + +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) contain the exact joint vectors, seeds, trajectory metrics, tracking metrics, 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,9 @@ 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 ``` 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). @@ -94,6 +103,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/crates/arm-lab-demo/src/bin/multi_query_bench.rs b/crates/arm-lab-demo/src/bin/multi_query_bench.rs index 82deaf2..655a1a0 100644 --- a/crates/arm-lab-demo/src/bin/multi_query_bench.rs +++ b/crates/arm-lab-demo/src/bin/multi_query_bench.rs @@ -2,10 +2,11 @@ //! //! This executable complements, rather than replaces, the single-trajectory //! controller robustness envelope. It evaluates five planner seeds on nine -//! fixed queries across all three shipped UR5e scenes. The canonical-seed -//! trajectory for every query is then replayed with position PD and position -//! PD plus desired-velocity feedforward, both on the nominal plant and on the -//! same fixed combined plant shift used by `robustness_bench`. +//! 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 @@ -698,12 +699,11 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { let mut report = format!( "# UR5e multi-scene, multi-query benchmark (simulation)\n\n\ - This deterministic extension evaluates **{} fixed queries across {} shipped MJCF scenes**. Five fixed planner seeds per query produce {} planning trials. The canonical-seed trajectory for each query is then replayed using two controller variants against the nominal plant and one fixed combined shift, producing {} tracking trials. Three queries have collision-blocked straight interpolants. **This is simulation evidence, not hardware validation or a sim-to-real guarantee.**\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\ The tracking pass 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. They were declared in code before this benchmark was executed.\n\n\ ## Planning and trajectory summary\n\n\ | Scene | Query | Direct interpolant | Planner success | Median plan (ms) | Cost range (rad) | Canonical trajectory |\n\ |---|---|:---:|:---:|---:|---:|---:|\n", - QUERIES.len(), SCENES.len(), planning.len(), tracking.len(), @@ -824,6 +824,35 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { ) .expect("format aggregate result"); + let velocity_failures: Vec<&TrackingRow> = tracking + .iter() + .filter(|row| row.controller == Controller::VelocityFf.name() && !row.pass) + .collect(); + if !velocity_failures.is_empty() + && velocity_failures.iter().all(|row| { + row.rms_joint_rad <= PASS_RMS_RAD + && row.max_joint_rad <= PASS_MAX_RAD + && row.final_joint_rad > PASS_FINAL_RAD + }) + { + let min_final = velocity_failures + .iter() + .map(|row| row.final_joint_rad) + .fold(f64::INFINITY, f64::min); + let max_final = velocity_failures + .iter() + .map(|row| row.final_joint_rad) + .fold(0.0f64, f64::max); + writeln!( + report, + "All {} velocity-feedforward misses pass the RMS and maximum-error gates but fail the unchanged final-hold gate, with final errors from {:.4} to {:.4} rad.", + velocity_failures.len(), + min_final, + max_final + ) + .expect("format failure boundary"); + } + 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\ @@ -865,4 +894,16 @@ mod tests { 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); + } } diff --git a/docs/multi_query_planning.csv b/docs/multi_query_planning.csv new file mode 100644 index 0000000..4c835b9 --- /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.12197000,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.12721000,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.11967000,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.11932000,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.12538000,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.09535000,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.09419000,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.10312000,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.09427000,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.09376000,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.12341000,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.12122900,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.12555000,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.12035000,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.12776000,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.16162400,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,8.21314300,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,5.17048500,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.51122300,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.86913400,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.12829000,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.12384000,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.12650000,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.12108900,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.12280000,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.11213000,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.10637000,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.17039900,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.11298000,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.11046000,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.37105900,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.13311800,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.69159100,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.19346400,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.34577600,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.25045500,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,6.06427400,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.26459500,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.83088500,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.50793400,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.13395000,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.13129900,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.13751000,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.13724000,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.13068000,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..12fc45b --- /dev/null +++ b/docs/multi_query_results.md @@ -0,0 +1,69 @@ +# 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.** + +The tracking pass 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. They were declared in code before this benchmark was executed. + +## Planning and trajectory summary + +| Scene | Query | Direct interpolant | Planner success | Median plan (ms) | Cost range (rad) | Canonical trajectory | +|---|---|:---:|:---:|---:|---:|---:| +| open_floor | positive_pan | free | 5/5 | 0.122 | 1.100--1.100 | 1267 samples / 2.53 s | +| open_floor | shoulder_elbow | free | 5/5 | 0.094 | 0.805--0.805 | 767 samples / 1.53 s | +| open_floor | wrist_reorientation | free | 5/5 | 0.123 | 1.058--1.058 | 903 samples / 1.80 s | +| offset_pillar | positive_pan | blocked | 5/5 | 6.869 | 1.803--2.541 | 1751 samples / 3.50 s | +| offset_pillar | negative_pan | free | 5/5 | 0.124 | 1.000--1.000 | 1176 samples / 2.35 s | +| offset_pillar | shoulder_elbow | free | 5/5 | 0.112 | 0.805--0.805 | 767 samples / 1.53 s | +| tabletop_pillar | cross_workspace | blocked | 5/5 | 4.346 | 1.518--2.206 | 1515 samples / 3.03 s | +| tabletop_pillar | reverse_cross_workspace | blocked | 5/5 | 5.831 | 1.475--1.715 | 1679 samples / 3.35 s | +| tabletop_pillar | wrist_reorientation | free | 5/5 | 0.134 | 1.058--1.058 | 903 samples / 1.80 s | + +## Tracking results + +Each cell is RMS / maximum / final six-joint L2 error in radians. `PASS` requires all three declared limits. + +| Scene | Query | Plant | Position PD | PD + velocity FF | +|---|---|---|---:|---:| +| open_floor | positive_pan | nominal | 0.0913 / 0.1095 / 0.0119 FAIL | 0.0117 / 0.0118 / 0.0116 PASS | +| open_floor | positive_pan | combined moderate shift | 0.0973 / 0.1163 / 0.0195 FAIL | 0.0195 / 0.0202 / 0.0193 PASS | +| open_floor | shoulder_elbow | nominal | 0.1117 / 0.1552 / 0.0173 FAIL | 0.0140 / 0.0176 / 0.0176 PASS | +| open_floor | shoulder_elbow | combined moderate shift | 0.1188 / 0.1643 / 0.0266 FAIL | 0.0222 / 0.0270 / 0.0271 FAIL | +| open_floor | wrist_reorientation | nominal | 0.1245 / 0.1640 / 0.0105 FAIL | 0.0109 / 0.0116 / 0.0098 PASS | +| open_floor | wrist_reorientation | combined moderate shift | 0.1339 / 0.1760 / 0.0187 FAIL | 0.0200 / 0.0215 / 0.0176 PASS | +| offset_pillar | positive_pan | nominal | 0.1409 / 0.1719 / 0.0111 FAIL | 0.0097 / 0.0121 / 0.0117 PASS | +| offset_pillar | positive_pan | combined moderate shift | 0.1498 / 0.1852 / 0.0187 FAIL | 0.0186 / 0.0241 / 0.0194 PASS | +| offset_pillar | negative_pan | nominal | 0.0897 / 0.1095 / 0.0120 FAIL | 0.0117 / 0.0119 / 0.0117 PASS | +| offset_pillar | negative_pan | combined moderate shift | 0.0957 / 0.1163 / 0.0196 FAIL | 0.0195 / 0.0200 / 0.0193 PASS | +| offset_pillar | shoulder_elbow | nominal | 0.1117 / 0.1552 / 0.0173 FAIL | 0.0140 / 0.0176 / 0.0176 PASS | +| offset_pillar | shoulder_elbow | combined moderate shift | 0.1188 / 0.1643 / 0.0266 FAIL | 0.0222 / 0.0270 / 0.0271 FAIL | +| tabletop_pillar | cross_workspace | nominal | 0.0998 / 0.1204 / 0.0152 FAIL | 0.0132 / 0.0151 / 0.0152 PASS | +| tabletop_pillar | cross_workspace | combined moderate shift | 0.1067 / 0.1278 / 0.0239 FAIL | 0.0218 / 0.0252 / 0.0240 FAIL | +| tabletop_pillar | reverse_cross_workspace | nominal | 0.1033 / 0.1213 / 0.0134 FAIL | 0.0129 / 0.0143 / 0.0131 PASS | +| tabletop_pillar | reverse_cross_workspace | combined moderate shift | 0.1108 / 0.1315 / 0.0221 FAIL | 0.0213 / 0.0241 / 0.0216 FAIL | +| tabletop_pillar | wrist_reorientation | nominal | 0.1245 / 0.1640 / 0.0105 FAIL | 0.0109 / 0.0116 / 0.0098 PASS | +| tabletop_pillar | wrist_reorientation | combined moderate shift | 0.1339 / 0.1760 / 0.0187 FAIL | 0.0200 / 0.0215 / 0.0176 PASS | + +## Aggregate result + +- Planner: 45/45 fixed-seed trials succeeded. +- Position PD: 0/18 tracking cases passed. +- PD + velocity feedforward: 14/18 tracking cases passed. + +All 4 velocity-feedforward misses pass the RMS and maximum-error gates but fail the unchanged final-hold gate, with final errors from 0.0216 to 0.0271 rad. + +## 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. Wall-clock planning times are machine- and load-dependent. +- 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. +- Collision checks remain discrete at 0.05 rad in joint-space L2. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk. There is no sensor noise, contact-rich grasping, or hardware experiment. + +## Reproduce + +```bash +cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write +``` + +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 and complete metrics). diff --git a/docs/multi_query_tracking.csv b/docs/multi_query_tracking.csv new file mode 100644 index 0000000..129db45 --- /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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,true +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,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,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,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,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,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,true From fff1dc2dda7a565348fb4274b0db50a9da30a382 Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 02:10:27 +0100 Subject: [PATCH 3/7] Audit executed contacts in multi-query benchmark --- .../arm-lab-demo/src/bin/multi_query_bench.rs | 440 +++++++++++++++--- crates/arm-lab/src/collision.rs | 83 +++- crates/arm-lab/src/lib.rs | 2 +- crates/arm-lab/tests/plan.rs | 30 ++ 4 files changed, 473 insertions(+), 82 deletions(-) diff --git a/crates/arm-lab-demo/src/bin/multi_query_bench.rs b/crates/arm-lab-demo/src/bin/multi_query_bench.rs index 655a1a0..8d7568a 100644 --- a/crates/arm-lab-demo/src/bin/multi_query_bench.rs +++ b/crates/arm-lab-demo/src/bin/multi_query_bench.rs @@ -11,6 +11,7 @@ //! ```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; @@ -49,6 +50,7 @@ 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; +const EXECUTION_CLEARANCE_M: f64 = 1e-3; const LIMITS: TrajLimits = TrajLimits { v_max: 0.55, a_max: 1.8, @@ -229,15 +231,84 @@ struct TrackingRow { 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 collision_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) + } + + fn max_clearance_violation_m(&self) -> f64 { + self.settle_collisions + .max_clearance_violation_m + .max(self.path_collisions.max_clearance_violation_m) + .max(self.hold_collisions.max_clearance_violation_m) + } +} + +#[derive(Clone, Default)] +struct CollisionPhaseMetrics { + steps: usize, + max_penetration_m: f64, + max_clearance_violation_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); + let violation = (checker.clearance - contact.distance_m).max(0.0); + self.max_penetration_m = self.max_penetration_m.max(penetration); + if violation > self.max_clearance_violation_m { + self.max_clearance_violation_m = violation; + 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 write_results = std::env::args().any(|arg| arg == "--write"); + 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()); @@ -345,12 +416,14 @@ fn main() { &trajectory, ); println!( - " {:<23} | {:<16} | rms {:>7.4} | max {:>7.4} | final {:>7.4} | {}", + " {:<23} | {:<16} | rms {:>7.4} | max {:>7.4} | final {:>7.4} | collisions {:>4} | pen {:>7.3} mm | {}", metrics.plant, metrics.controller, metrics.rms_joint_rad, metrics.max_joint_rad, metrics.final_joint_rad, + metrics.collision_steps(), + 1e3 * metrics.max_penetration_m(), if metrics.pass { "PASS" } else { "FAIL" } ); tracking_rows.push(metrics); @@ -363,23 +436,30 @@ fn main() { .iter() .filter(|row| row.seed == CANONICAL_SEED && !row.direct_free) .count(); - let planner_successes = planning_rows + let direct_trials = planning_rows.iter().filter(|row| row.direct_free).count(); + let direct_successes = planning_rows .iter() - .filter(|row| row.status == PlanStatus::Success) + .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: {planner_successes}/{} planner successes; {blocked_queries}/{} blocked-direct queries; {velocity_passes}/{} velocity-FF passes", - planning_rows.len(), + "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() ); - if write_results { - write_artifacts(&planning_rows, &tracking_rows); + match mode { + Mode::Run => {} + Mode::Write => write_artifacts(&planning_rows, &tracking_rows), + Mode::Check => check_artifacts(&planning_rows, &tracking_rows), } } @@ -494,6 +574,11 @@ fn run_tracking_case( 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.clearance = EXECUTION_CLEARANCE_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(), @@ -504,6 +589,7 @@ fn run_tracking_case( 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; @@ -523,6 +609,7 @@ fn run_tracking_case( ); 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; @@ -543,11 +630,15 @@ fn run_tracking_case( 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 pass = - rms_joint <= PASS_RMS_RAD && max_joint <= PASS_MAX_RAD && final_joint <= PASS_FINAL_RAD; + let collision_steps = settle_collisions.steps + path_collisions.steps + hold_collisions.steps; + let pass = rms_joint <= PASS_RMS_RAD + && max_joint <= PASS_MAX_RAD + && final_joint <= PASS_FINAL_RAD + && collision_steps == 0; TrackingRow { scene: scene.name, @@ -564,6 +655,9 @@ fn run_tracking_case( 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, } } @@ -619,6 +713,30 @@ fn l2(a: &[f64], b: &[f64]) -> f64 { .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 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_clearance_violation_m + .total_cmp(&right.max_clearance_violation_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", @@ -635,10 +753,13 @@ fn format_joint_vector(q: &[f64]) -> String { .join(";") } -fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { - let docs = Path::new(env!("CARGO_MANIFEST_DIR")).join("../../docs"); - std::fs::create_dir_all(&docs).expect("create docs directory"); +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", ); @@ -665,16 +786,13 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { ) .expect("format planning CSV"); } - std::fs::write(docs.join("multi_query_planning.csv"), planning_csv) - .expect("write 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,pass\n", + "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_collision_steps,settle_max_penetration_m,settle_max_clearance_violation_m,settle_worst_contact_distance_m,settle_worst_contact,path_collision_steps,path_max_penetration_m,path_max_clearance_violation_m,path_worst_contact_distance_m,path_worst_contact,hold_collision_steps,hold_max_penetration_m,hold_max_clearance_violation_m,hold_worst_contact_distance_m,hold_worst_contact,collision_steps,max_penetration_m,max_clearance_violation_m,pass\n", ); for row in tracking { writeln!( tracking_csv, - "{},{},{},{},{},{},{},{},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{}", + "{},{},{},{},{},{},{},{},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{},{:.8},{:.8},{},{},{},{:.8},{:.8},{},{},{},{:.8},{:.8},{},{},{},{:.8},{:.8},{}", row.scene, row.scene_file, row.query, @@ -690,36 +808,64 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { row.max_ee_pos_m, row.peak_force_fraction, row.saturated_step_fraction, + row.settle_collisions.steps, + row.settle_collisions.max_penetration_m, + row.settle_collisions.max_clearance_violation_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, + row.path_collisions.max_clearance_violation_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, + row.hold_collisions.max_clearance_violation_m, + format_optional_distance(row.hold_collisions.worst_contact_distance_m), + row.hold_collisions.worst_contact.as_deref().unwrap_or(""), + row.collision_steps(), + row.max_penetration_m(), + row.max_clearance_violation_m(), row.pass ) .expect("format tracking CSV"); } - std::fs::write(docs.join("multi_query_tracking.csv"), tracking_csv) - .expect("write 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\ - The tracking pass 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. They were declared in code before this benchmark was executed.\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 now passes only with **zero robot-collision steps** across settling, path execution, and hold under the repository's {:.3}-m clearance semantics. Signed contact distance, actual penetration, clearance violation, and worst geom pair are retained in the raw CSV.\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 | Median plan (ms) | Cost range (rad) | Canonical trajectory |\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 + PASS_FINAL_RAD, + EXECUTION_CLEARANCE_M ); for query in QUERIES { let scene = SCENES[query.scene]; - let mut rows: Vec<&PlanningRow> = planning + let rows: Vec<&PlanningRow> = planning .iter() .filter(|row| row.scene == scene.name && row.query == query.name) .collect(); - rows.sort_by(|a, b| a.elapsed_ms.total_cmp(&b.elapsed_ms)); let successes = rows .iter() .filter(|row| row.status == PlanStatus::Success) @@ -738,7 +884,7 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { .expect("canonical planning row"); writeln!( report, - "| {} | {} | {} | {}/{} | {:.3} | {:.3}--{:.3} | {} samples / {:.2} s |", + "| {} | {} | {} | {}/{} | {:.3}--{:.3} | {} samples / {:.2} s |", scene.name, query.name, if canonical.direct_free { @@ -748,7 +894,6 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { }, successes, rows.len(), - rows[rows.len() / 2].elapsed_ms, min_cost, max_cost, canonical.trajectory_samples, @@ -759,7 +904,7 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { report.push_str( "\n## Tracking results\n\n\ - Each cell is RMS / maximum / final six-joint L2 error in radians. `PASS` requires all three declared limits.\n\n\ + Each cell is RMS / maximum / final six-joint L2 error in radians, followed by collision steps in settle/path/hold and maximum actual penetration. `PASS` requires all three numeric limits and zero collision steps.\n\n\ | Scene | Query | Plant | Position PD | PD + velocity FF |\n\ |---|---|---|---:|---:|\n", ); @@ -786,27 +931,31 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { .expect("velocity row"); writeln!( report, - "| {} | {} | {} | {:.4} / {:.4} / {:.4} {} | {:.4} / {:.4} / {:.4} {} |", + "| {} | {} | {} | {:.4}/{:.4}/{:.4}; c {}/{}/{}; pen {:.3} mm {} | {:.4}/{:.4}/{:.4}; c {}/{}/{}; 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 planner_passes = planning - .iter() - .filter(|row| row.status == PlanStatus::Success) - .count(); let position_passes = tracking .iter() .filter(|row| row.controller == Controller::Position.name() && row.pass) @@ -815,65 +964,195 @@ fn write_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) { .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 collision_cases: Vec<&TrackingRow> = tracking + .iter() + .filter(|row| row.collision_steps() > 0) + .collect(); + let total_collision_steps: usize = collision_cases + .iter() + .map(|row| row.collision_steps()) + .sum(); + let max_penetration_m = collision_cases + .iter() + .map(|row| row.max_penetration_m()) + .fold(0.0f64, f64::max); + let max_clearance_violation_m = collision_cases + .iter() + .map(|row| row.max_clearance_violation_m()) + .fold(0.0f64, f64::max); writeln!( report, - "\n## Aggregate result\n\n- Planner: {planner_passes}/{} fixed-seed trials succeeded.\n- Position PD: {position_passes}/{} tracking cases passed.\n- PD + velocity feedforward: {velocity_passes}/{} tracking cases passed.\n", - planning.len(), + "\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-collision requirement.\n- PD + velocity feedforward: {velocity_numeric_passes}/{} meet the numeric tracking gates; {velocity_passes}/{} pass after the zero-collision requirement.\n- Executed-contact audit: {}/{} cases have robot collision steps; {total_collision_steps} total phase-steps, maximum actual penetration {:.6} m, maximum {:.3}-m-clearance violation {:.6} m.\n", + tracking.len() / 2, tracking.len() / 2, - tracking.len() / 2 + tracking.len() / 2, + tracking.len() / 2, + collision_cases.len(), + tracking.len(), + max_penetration_m, + EXECUTION_CLEARANCE_M, + max_clearance_violation_m ) .expect("format aggregate result"); - let velocity_failures: Vec<&TrackingRow> = tracking - .iter() - .filter(|row| row.controller == Controller::VelocityFf.name() && !row.pass) - .collect(); - if !velocity_failures.is_empty() - && velocity_failures.iter().all(|row| { - row.rms_joint_rad <= PASS_RMS_RAD - && row.max_joint_rad <= PASS_MAX_RAD - && row.final_joint_rad > PASS_FINAL_RAD - }) - { - let min_final = velocity_failures - .iter() - .map(|row| row.final_joint_rad) - .fold(f64::INFINITY, f64::min); - let max_final = velocity_failures - .iter() - .map(|row| row.final_joint_rad) - .fold(0.0f64, f64::max); - writeln!( - report, - "All {} velocity-feedforward misses pass the RMS and maximum-error gates but fail the unchanged final-hold gate, with final errors from {:.4} to {:.4} rad.", - velocity_failures.len(), - min_final, - max_final - ) - .expect("format failure boundary"); + if !collision_cases.is_empty() { + report.push_str( + "\n## Executed collision cases\n\n\ + Collision steps are shown as settle/path/hold. The worst contact is selected by maximum clearance violation; signed distance below zero is actual penetration.\n\n\ + | Scene | Query | Plant | Controller | Steps S/P/H | Max penetration (m) | Max clearance violation (m) | Worst phase / signed distance / geom pair |\n\ + |---|---|---|---|---:|---:|---:|---|\n", + ); + for row in &collision_cases { + let (phase, worst) = worst_collision(row); + writeln!( + report, + "| {} | {} | {} | {} | {}/{}/{} | {:.8} | {:.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(), + row.max_clearance_violation_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 collision-negative 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. Wall-clock planning times are machine- and load-dependent.\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\ - - Collision checks remain discrete at 0.05 rad in joint-space L2. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk. There is no sensor noise, contact-rich grasping, or hardware experiment.\n\n\ + - Planner collision checks remain discrete at 0.05 rad in joint-space L2. Executed collision checks sample each 2-ms simulation state in settle, path, and hold; neither is a continuous swept-volume certificate. 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 and complete metrics).\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 collision counts/depths, and worst contact identities).\n", ); - std::fs::write(docs.join("multi_query_results.md"), report).expect("write Markdown report"); + 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::*; @@ -906,4 +1185,29 @@ mod tests { } 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 collision_violation_is_penetration_plus_clearance() { + let signed_distance: f64 = -0.004; + let penetration = (-signed_distance).max(0.0); + let violation = (EXECUTION_CLEARANCE_M - signed_distance).max(0.0); + assert!((penetration - 0.004).abs() < 1e-12); + assert!((violation - 0.005).abs() < 1e-12); + } } diff --git a/crates/arm-lab/src/collision.rs b/crates/arm-lab/src/collision.rs index 9095256..4fff119 100644 --- a/crates/arm-lab/src/collision.rs +++ b/crates/arm-lab/src/collision.rs @@ -16,6 +16,30 @@ use mujoco_rs::prelude::*; use crate::chain::Chain; +/// A robot-involved MuJoCo contact below the checker's clearance threshold. +/// +/// `distance_m` is MuJoCo's signed contact distance: negative values are +/// penetration, while positive values below [`CollisionChecker::clearance`] +/// violate the requested safety margin without geometric penetration. +#[derive(Debug, Clone, PartialEq)] +pub struct RobotContact { + pub geom1_id: i32, + pub geom2_id: i32, + pub geom1_name: String, + pub geom2_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.geom2_name, self.geom2_id + ) + } +} + /// MuJoCo-backed collision oracle for a serial chain. pub struct CollisionChecker> { data: MjData, @@ -77,22 +101,45 @@ 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.clearance && self.contact_involves_robot(contact.geom) + }) + } + + /// Robot-involved contacts whose signed distance is below `clearance`. + /// + /// 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.clearance && 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]), + 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 +152,13 @@ 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) +} 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/plan.rs b/crates/arm-lab/tests/plan.rs index 5bf96c8..f707f48 100644 --- a/crates/arm-lab/tests/plan.rs +++ b/crates/arm-lab/tests/plan.rs @@ -175,6 +175,36 @@ 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); + 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 < checker.clearance + && !contact.geom1_name.is_empty() + && !contact.geom2_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(); From 8df51bb82d248ca116a81b6fc4286e7d816dd8a9 Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 02:14:22 +0100 Subject: [PATCH 4/7] Gate multi-query evidence on executed collisions --- .github/workflows/ci.yml | 1 + README.md | 7 +- .../arm-lab-demo/src/bin/multi_query_bench.rs | 24 ++++- crates/arm-lab/src/collision.rs | 24 ++++- crates/arm-lab/tests/plan.rs | 2 + docs/multi_query_planning.csv | 90 ++++++++--------- docs/multi_query_results.md | 96 +++++++++++-------- docs/multi_query_tracking.csv | 74 +++++++------- 8 files changed, 190 insertions(+), 128 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4c556de..be4befb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -39,6 +39,7 @@ jobs: 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 f48610f..1a57f77 100644 --- a/README.md +++ b/README.md @@ -37,9 +37,9 @@ The generated [full results](docs/robustness_results.md) and [raw CSV](docs/robu ## 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 tracking thresholds. Selected joint pairs are deliberately repeated across scenes to isolate geometry effects. Across five fixed planner seeds per fixture, all **45/45** plans succeeded; three fixtures had collision-blocked straight interpolants. Position PD passed **0/18** nominal-plus-combined-shift tracking cases. Adding desired-velocity feedforward passed **9/9 nominal** and **5/9 combined-shift** cases. The four combined-shift misses are retained in the raw evidence: their final errors exceed the 0.02-rad hold gate even though RMS and maximum errors remain inside their gates. +`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 then checked after every settling, path, and hold step using the same 1-mm robot-contact semantics as the planner; a case passes only with zero collision steps. -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) contain the exact joint vectors, seeds, trajectory metrics, tracking metrics, 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. +Position PD passes **0/18** tracking cases. Desired-velocity feedforward meets the numeric gates in **14/18** cases and passes the full collision-aware gate in **13/18**. All four executed collision cases are 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 collision-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) include exact joint vectors, seeds, trajectory metrics, per-phase collision 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 @@ -81,6 +81,9 @@ 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). diff --git a/crates/arm-lab-demo/src/bin/multi_query_bench.rs b/crates/arm-lab-demo/src/bin/multi_query_bench.rs index 8d7568a..35799a1 100644 --- a/crates/arm-lab-demo/src/bin/multi_query_bench.rs +++ b/crates/arm-lab-demo/src/bin/multi_query_bench.rs @@ -635,10 +635,7 @@ fn run_tracking_case( let final_joint = l2(&read_q(&data, chain), q_goal); let rms_joint = (sum_sq / samples as f64).sqrt(); let collision_steps = settle_collisions.steps + path_collisions.steps + hold_collisions.steps; - let pass = rms_joint <= PASS_RMS_RAD - && max_joint <= PASS_MAX_RAD - && final_joint <= PASS_FINAL_RAD - && collision_steps == 0; + let pass = case_pass(rms_joint, max_joint, final_joint, collision_steps); TrackingRow { scene: scene.name, @@ -719,6 +716,13 @@ fn numeric_gates_pass(row: &TrackingRow) -> bool { && row.final_joint_rad <= PASS_FINAL_RAD } +fn case_pass(rms_joint: f64, max_joint: f64, final_joint: f64, collision_steps: usize) -> bool { + rms_joint <= PASS_RMS_RAD + && max_joint <= PASS_MAX_RAD + && final_joint <= PASS_FINAL_RAD + && collision_steps == 0 +} + fn worst_collision(row: &TrackingRow) -> (&'static str, &CollisionPhaseMetrics) { [ ("settle", &row.settle_collisions), @@ -1210,4 +1214,16 @@ mod tests { assert!((penetration - 0.004).abs() < 1e-12); assert!((violation - 0.005).abs() < 1e-12); } + + #[test] + fn collision_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 4fff119..bbe84ab 100644 --- a/crates/arm-lab/src/collision.rs +++ b/crates/arm-lab/src/collision.rs @@ -27,6 +27,8 @@ pub struct RobotContact { pub geom2_id: i32, pub geom1_name: String, pub geom2_name: String, + pub body1_name: String, + pub body2_name: String, pub distance_m: f64, } @@ -34,8 +36,13 @@ impl RobotContact { /// Stable human-readable identity for raw benchmark artifacts. pub fn identity(&self) -> String { format!( - "{}[{}] vs {}[{}]", - self.geom1_name, self.geom1_id, self.geom2_name, self.geom2_id + "{}[{}]@{} vs {}[{}]@{}", + self.geom1_name, + self.geom1_id, + self.body1_name, + self.geom2_name, + self.geom2_id, + self.body2_name ) } } @@ -126,6 +133,8 @@ impl> CollisionChecker { 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() @@ -162,3 +171,14 @@ fn geom_name(model: &MjModel, id: i32) -> String { .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/tests/plan.rs b/crates/arm-lab/tests/plan.rs index f707f48..ea5dc93 100644 --- a/crates/arm-lab/tests/plan.rs +++ b/crates/arm-lab/tests/plan.rs @@ -197,6 +197,8 @@ fn collision_audit_matches_boolean_and_identifies_pillar() { contact.distance_m < checker.clearance && !contact.geom1_name.is_empty() && !contact.geom2_name.is_empty() + && !contact.body1_name.is_empty() + && !contact.body2_name.is_empty() })); assert!( contacts diff --git a/docs/multi_query_planning.csv b/docs/multi_query_planning.csv index 4c835b9..6300960 100644 --- a/docs/multi_query_planning.csv +++ b/docs/multi_query_planning.csv @@ -1,46 +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.12197000,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.12721000,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.11967000,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.11932000,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.12538000,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.09535000,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.09419000,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.10312000,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.09427000,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.09376000,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.12341000,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.12122900,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.12555000,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.12035000,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.12776000,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.16162400,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,8.21314300,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,5.17048500,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.51122300,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.86913400,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.12829000,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.12384000,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.12650000,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.12108900,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.12280000,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.11213000,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.10637000,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.17039900,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.11298000,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.11046000,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.37105900,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.13311800,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.69159100,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.19346400,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.34577600,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.25045500,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,6.06427400,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.26459500,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.83088500,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.50793400,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.13395000,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.13129900,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.13751000,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.13724000,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.13068000,0,2,2,23,1.05830052,903,1.80328283 +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.12429000,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.12327000,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.12346000,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.12200000,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.12163000,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.09689000,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.09466000,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.09391900,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.09342000,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.09335000,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.12424000,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.12570900,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.12300000,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.12283900,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.12298000,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.93587900,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,5.93811100,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,4.98799300,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.43564800,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.75752900,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.12946000,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.12333900,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.12260000,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.12263900,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.12287000,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.10958900,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.11385000,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.10687000,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.10672000,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.10679000,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.37385800,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.14670700,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.94178400,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.10870000,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.37058300,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.07293200,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,5.77112000,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.42939100,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.75411000,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.38588000,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.13495900,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.13814000,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.12981900,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.12978000,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.13728000,0,2,2,23,1.05830052,903,1.80328283 diff --git a/docs/multi_query_results.md b/docs/multi_query_results.md index 12fc45b..317b2bf 100644 --- a/docs/multi_query_results.md +++ b/docs/multi_query_results.md @@ -2,68 +2,88 @@ 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.** -The tracking pass 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. They were declared in code before this benchmark was executed. +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 now passes only with **zero robot-collision steps** across settling, path execution, and hold under the repository's 0.001-m clearance semantics. Signed contact distance, actual penetration, clearance violation, and worst geom pair are retained in the raw CSV. + +`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 | Median plan (ms) | Cost range (rad) | Canonical trajectory | -|---|---|:---:|:---:|---:|---:|---:| -| open_floor | positive_pan | free | 5/5 | 0.122 | 1.100--1.100 | 1267 samples / 2.53 s | -| open_floor | shoulder_elbow | free | 5/5 | 0.094 | 0.805--0.805 | 767 samples / 1.53 s | -| open_floor | wrist_reorientation | free | 5/5 | 0.123 | 1.058--1.058 | 903 samples / 1.80 s | -| offset_pillar | positive_pan | blocked | 5/5 | 6.869 | 1.803--2.541 | 1751 samples / 3.50 s | -| offset_pillar | negative_pan | free | 5/5 | 0.124 | 1.000--1.000 | 1176 samples / 2.35 s | -| offset_pillar | shoulder_elbow | free | 5/5 | 0.112 | 0.805--0.805 | 767 samples / 1.53 s | -| tabletop_pillar | cross_workspace | blocked | 5/5 | 4.346 | 1.518--2.206 | 1515 samples / 3.03 s | -| tabletop_pillar | reverse_cross_workspace | blocked | 5/5 | 5.831 | 1.475--1.715 | 1679 samples / 3.35 s | -| tabletop_pillar | wrist_reorientation | free | 5/5 | 0.134 | 1.058--1.058 | 903 samples / 1.80 s | +| 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. `PASS` requires all three declared limits. +Each cell is RMS / maximum / final six-joint L2 error in radians, followed by collision steps in settle/path/hold and maximum actual penetration. `PASS` requires all three numeric limits and zero collision steps. | Scene | Query | Plant | Position PD | PD + velocity FF | |---|---|---|---:|---:| -| open_floor | positive_pan | nominal | 0.0913 / 0.1095 / 0.0119 FAIL | 0.0117 / 0.0118 / 0.0116 PASS | -| open_floor | positive_pan | combined moderate shift | 0.0973 / 0.1163 / 0.0195 FAIL | 0.0195 / 0.0202 / 0.0193 PASS | -| open_floor | shoulder_elbow | nominal | 0.1117 / 0.1552 / 0.0173 FAIL | 0.0140 / 0.0176 / 0.0176 PASS | -| open_floor | shoulder_elbow | combined moderate shift | 0.1188 / 0.1643 / 0.0266 FAIL | 0.0222 / 0.0270 / 0.0271 FAIL | -| open_floor | wrist_reorientation | nominal | 0.1245 / 0.1640 / 0.0105 FAIL | 0.0109 / 0.0116 / 0.0098 PASS | -| open_floor | wrist_reorientation | combined moderate shift | 0.1339 / 0.1760 / 0.0187 FAIL | 0.0200 / 0.0215 / 0.0176 PASS | -| offset_pillar | positive_pan | nominal | 0.1409 / 0.1719 / 0.0111 FAIL | 0.0097 / 0.0121 / 0.0117 PASS | -| offset_pillar | positive_pan | combined moderate shift | 0.1498 / 0.1852 / 0.0187 FAIL | 0.0186 / 0.0241 / 0.0194 PASS | -| offset_pillar | negative_pan | nominal | 0.0897 / 0.1095 / 0.0120 FAIL | 0.0117 / 0.0119 / 0.0117 PASS | -| offset_pillar | negative_pan | combined moderate shift | 0.0957 / 0.1163 / 0.0196 FAIL | 0.0195 / 0.0200 / 0.0193 PASS | -| offset_pillar | shoulder_elbow | nominal | 0.1117 / 0.1552 / 0.0173 FAIL | 0.0140 / 0.0176 / 0.0176 PASS | -| offset_pillar | shoulder_elbow | combined moderate shift | 0.1188 / 0.1643 / 0.0266 FAIL | 0.0222 / 0.0270 / 0.0271 FAIL | -| tabletop_pillar | cross_workspace | nominal | 0.0998 / 0.1204 / 0.0152 FAIL | 0.0132 / 0.0151 / 0.0152 PASS | -| tabletop_pillar | cross_workspace | combined moderate shift | 0.1067 / 0.1278 / 0.0239 FAIL | 0.0218 / 0.0252 / 0.0240 FAIL | -| tabletop_pillar | reverse_cross_workspace | nominal | 0.1033 / 0.1213 / 0.0134 FAIL | 0.0129 / 0.0143 / 0.0131 PASS | -| tabletop_pillar | reverse_cross_workspace | combined moderate shift | 0.1108 / 0.1315 / 0.0221 FAIL | 0.0213 / 0.0241 / 0.0216 FAIL | -| tabletop_pillar | wrist_reorientation | nominal | 0.1245 / 0.1640 / 0.0105 FAIL | 0.0109 / 0.0116 / 0.0098 PASS | -| tabletop_pillar | wrist_reorientation | combined moderate shift | 0.1339 / 0.1760 / 0.0187 FAIL | 0.0200 / 0.0215 / 0.0176 PASS | +| open_floor | positive_pan | nominal | 0.0913/0.1095/0.0119; c 0/0/0; pen 0.000 mm FAIL | 0.0117/0.0118/0.0116; c 0/0/0; pen 0.000 mm PASS | +| open_floor | positive_pan | combined moderate shift | 0.0973/0.1163/0.0195; c 0/0/0; pen 0.000 mm FAIL | 0.0195/0.0202/0.0193; c 0/0/0; pen 0.000 mm PASS | +| open_floor | shoulder_elbow | nominal | 0.1117/0.1552/0.0173; c 0/0/0; pen 0.000 mm FAIL | 0.0140/0.0176/0.0176; c 0/0/0; pen 0.000 mm PASS | +| open_floor | shoulder_elbow | combined moderate shift | 0.1188/0.1643/0.0266; c 0/0/0; pen 0.000 mm FAIL | 0.0222/0.0270/0.0271; c 0/0/0; pen 0.000 mm FAIL | +| open_floor | wrist_reorientation | nominal | 0.1245/0.1640/0.0105; c 0/0/0; pen 0.000 mm FAIL | 0.0109/0.0116/0.0098; c 0/0/0; pen 0.000 mm PASS | +| open_floor | wrist_reorientation | combined moderate shift | 0.1339/0.1760/0.0187; c 0/0/0; pen 0.000 mm FAIL | 0.0200/0.0215/0.0176; c 0/0/0; pen 0.000 mm PASS | +| offset_pillar | positive_pan | nominal | 0.1409/0.1719/0.0111; c 0/0/0; pen 0.000 mm FAIL | 0.0097/0.0121/0.0117; c 0/0/0; pen 0.000 mm PASS | +| offset_pillar | positive_pan | combined moderate shift | 0.1498/0.1852/0.0187; c 0/0/0; pen 0.000 mm FAIL | 0.0186/0.0241/0.0194; c 0/0/0; pen 0.000 mm PASS | +| offset_pillar | negative_pan | nominal | 0.0897/0.1095/0.0120; c 0/0/0; pen 0.000 mm FAIL | 0.0117/0.0119/0.0117; c 0/0/0; pen 0.000 mm PASS | +| offset_pillar | negative_pan | combined moderate shift | 0.0957/0.1163/0.0196; c 0/0/0; pen 0.000 mm FAIL | 0.0195/0.0200/0.0193; c 0/0/0; pen 0.000 mm PASS | +| offset_pillar | shoulder_elbow | nominal | 0.1117/0.1552/0.0173; c 0/0/0; pen 0.000 mm FAIL | 0.0140/0.0176/0.0176; c 0/0/0; pen 0.000 mm PASS | +| offset_pillar | shoulder_elbow | combined moderate shift | 0.1188/0.1643/0.0266; c 0/0/0; pen 0.000 mm FAIL | 0.0222/0.0270/0.0271; c 0/0/0; pen 0.000 mm FAIL | +| tabletop_pillar | cross_workspace | nominal | 0.0998/0.1204/0.0152; c 0/0/0; pen 0.000 mm FAIL | 0.0132/0.0151/0.0152; c 0/0/0; pen 0.000 mm PASS | +| tabletop_pillar | cross_workspace | combined moderate shift | 0.1067/0.1278/0.0239; c 0/0/0; pen 0.000 mm FAIL | 0.0218/0.0252/0.0240; c 0/0/0; pen 0.000 mm FAIL | +| tabletop_pillar | reverse_cross_workspace | nominal | 0.1033/0.1213/0.0134; c 0/35/0; pen 0.055 mm FAIL | 0.0129/0.0143/0.0131; c 0/25/0; pen 0.050 mm FAIL | +| tabletop_pillar | reverse_cross_workspace | combined moderate shift | 0.1108/0.1315/0.0221; c 0/72/0; pen 0.079 mm FAIL | 0.0213/0.0241/0.0216; c 0/60/0; pen 0.079 mm FAIL | +| tabletop_pillar | wrist_reorientation | nominal | 0.1245/0.1640/0.0105; c 0/0/0; pen 0.000 mm FAIL | 0.0109/0.0116/0.0098; c 0/0/0; pen 0.000 mm PASS | +| tabletop_pillar | wrist_reorientation | combined moderate shift | 0.1339/0.1760/0.0187; c 0/0/0; pen 0.000 mm FAIL | 0.0200/0.0215/0.0176; c 0/0/0; pen 0.000 mm PASS | ## Aggregate result -- Planner: 45/45 fixed-seed trials succeeded. -- Position PD: 0/18 tracking cases passed. -- PD + velocity feedforward: 14/18 tracking cases passed. +- 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-collision requirement. +- PD + velocity feedforward: 14/18 meet the numeric tracking gates; 13/18 pass after the zero-collision requirement. +- Executed-contact audit: 4/36 cases have robot collision steps; 192 total phase-steps, maximum actual penetration 0.000079 m, maximum 0.001-m-clearance violation 0.001079 m. + + +## Executed collision cases + +Collision steps are shown as settle/path/hold. The worst contact is selected by maximum clearance violation; signed distance below zero is actual penetration. + +| Scene | Query | Plant | Controller | Steps S/P/H | Max penetration (m) | Max clearance violation (m) | Worst phase / signed distance / geom pair | +|---|---|---|---|---:|---:|---:|---| +| tabletop_pillar | reverse_cross_workspace | nominal | position PD | 0/35/0 | 0.00005480 | 0.00105480 | 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 | 0.00105013 | 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 | 0.00107925 | 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 | 0.00107922 | path / -0.00007922 / geom#27[27]@wrist_2_link vs pillar[31]@pillar | -All 4 velocity-feedforward misses pass the RMS and maximum-error gates but fail the unchanged final-hold gate, with final errors from 0.0216 to 0.0271 rad. +The fixed `tabletop_pillar/reverse_cross_workspace` fixture is retained in full, including any collision-negative 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. Wall-clock planning times are machine- and load-dependent. +- 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. -- Collision checks remain discrete at 0.05 rad in joint-space L2. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk. There is no sensor noise, contact-rich grasping, or hardware experiment. +- Planner collision checks remain discrete at 0.05 rad in joint-space L2. Executed collision checks sample each 2-ms simulation state in settle, path, and hold; neither is a continuous swept-volume certificate. 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 and complete metrics). +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 collision counts/depths, and worst contact identities). diff --git a/docs/multi_query_tracking.csv b/docs/multi_query_tracking.csv index 129db45..cb5d302 100644 --- a/docs/multi_query_tracking.csv +++ b/docs/multi_query_tracking.csv @@ -1,37 +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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,true -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,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,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,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,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,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,true +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_collision_steps,settle_max_penetration_m,settle_max_clearance_violation_m,settle_worst_contact_distance_m,settle_worst_contact,path_collision_steps,path_max_penetration_m,path_max_clearance_violation_m,path_worst_contact_distance_m,path_worst_contact,hold_collision_steps,hold_max_penetration_m,hold_max_clearance_violation_m,hold_worst_contact_distance_m,hold_worst_contact,collision_steps,max_penetration_m,max_clearance_violation_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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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,0.00000000,,,35,0.00005480,0.00105480,-0.00005480,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,35,0.00005480,0.00105480,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,0.00000000,,,25,0.00005013,0.00105013,-0.00005013,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,25,0.00005013,0.00105013,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,0.00000000,,,72,0.00007925,0.00107925,-0.00007925,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,72,0.00007925,0.00107925,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,0.00000000,,,60,0.00007922,0.00107922,-0.00007922,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,60,0.00007922,0.00107922,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,true From a23630d79f68e771f874c6016511aa3a52461ce3 Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 02:27:33 +0100 Subject: [PATCH 5/7] Define collision audit as sampled penetration --- README.md | 6 +- crates/arm-lab-demo/src/bin/demo3.rs | 13 ++- .../arm-lab-demo/src/bin/multi_query_bench.rs | 109 +++++++----------- crates/arm-lab/src/collision.rs | 34 ++++-- crates/arm-lab/tests/pickplace.rs | 5 +- crates/arm-lab/tests/plan.rs | 3 +- 6 files changed, 81 insertions(+), 89 deletions(-) diff --git a/README.md b/README.md index 1a57f77..579cc28 100644 --- a/README.md +++ b/README.md @@ -37,9 +37,9 @@ The generated [full results](docs/robustness_results.md) and [raw CSV](docs/robu ## 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 then checked after every settling, path, and hold step using the same 1-mm robot-contact semantics as the planner; a case passes only with zero collision steps. +`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 passes **0/18** tracking cases. Desired-velocity feedforward meets the numeric gates in **14/18** cases and passes the full collision-aware gate in **13/18**. All four executed collision cases are 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 collision-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) include exact joint vectors, seeds, trajectory metrics, per-phase collision 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. +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 @@ -97,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 diff --git a/crates/arm-lab-demo/src/bin/demo3.rs b/crates/arm-lab-demo/src/bin/demo3.rs index 58c8327..6838192 100644 --- a/crates/arm-lab-demo/src/bin/demo3.rs +++ b/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 { - 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 index 35799a1..e3a810d 100644 --- a/crates/arm-lab-demo/src/bin/multi_query_bench.rs +++ b/crates/arm-lab-demo/src/bin/multi_query_bench.rs @@ -50,7 +50,9 @@ 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; -const EXECUTION_CLEARANCE_M: f64 = 1e-3; +/// 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, @@ -238,7 +240,7 @@ struct TrackingRow { } impl TrackingRow { - fn collision_steps(&self) -> usize { + fn penetration_steps(&self) -> usize { self.settle_collisions.steps + self.path_collisions.steps + self.hold_collisions.steps } @@ -248,20 +250,12 @@ impl TrackingRow { .max(self.path_collisions.max_penetration_m) .max(self.hold_collisions.max_penetration_m) } - - fn max_clearance_violation_m(&self) -> f64 { - self.settle_collisions - .max_clearance_violation_m - .max(self.path_collisions.max_clearance_violation_m) - .max(self.hold_collisions.max_clearance_violation_m) - } } #[derive(Clone, Default)] struct CollisionPhaseMetrics { steps: usize, max_penetration_m: f64, - max_clearance_violation_m: f64, worst_contact_distance_m: Option, worst_contact: Option, } @@ -275,10 +269,8 @@ impl CollisionPhaseMetrics { self.steps += 1; for contact in contacts { let penetration = (-contact.distance_m).max(0.0); - let violation = (checker.clearance - contact.distance_m).max(0.0); - self.max_penetration_m = self.max_penetration_m.max(penetration); - if violation > self.max_clearance_violation_m { - self.max_clearance_violation_m = violation; + 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()); } @@ -416,13 +408,13 @@ fn main() { &trajectory, ); println!( - " {:<23} | {:<16} | rms {:>7.4} | max {:>7.4} | final {:>7.4} | collisions {:>4} | pen {:>7.3} mm | {}", + " {:<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.collision_steps(), + metrics.penetration_steps(), 1e3 * metrics.max_penetration_m(), if metrics.pass { "PASS" } else { "FAIL" } ); @@ -575,7 +567,7 @@ fn run_tracking_case( 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.clearance = EXECUTION_CLEARANCE_M; + 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(); @@ -634,8 +626,8 @@ fn run_tracking_case( } let final_joint = l2(&read_q(&data, chain), q_goal); let rms_joint = (sum_sq / samples as f64).sqrt(); - let collision_steps = settle_collisions.steps + path_collisions.steps + hold_collisions.steps; - let pass = case_pass(rms_joint, max_joint, final_joint, collision_steps); + 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, @@ -716,11 +708,11 @@ fn numeric_gates_pass(row: &TrackingRow) -> bool { && row.final_joint_rad <= PASS_FINAL_RAD } -fn case_pass(rms_joint: f64, max_joint: f64, final_joint: f64, collision_steps: usize) -> bool { +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 - && collision_steps == 0 + && penetration_steps == 0 } fn worst_collision(row: &TrackingRow) -> (&'static str, &CollisionPhaseMetrics) { @@ -730,10 +722,7 @@ fn worst_collision(row: &TrackingRow) -> (&'static str, &CollisionPhaseMetrics) ("hold", &row.hold_collisions), ] .into_iter() - .max_by(|(_, left), (_, right)| { - left.max_clearance_violation_m - .total_cmp(&right.max_clearance_violation_m) - }) + .max_by(|(_, left), (_, right)| left.max_penetration_m.total_cmp(&right.max_penetration_m)) .expect("three collision phases") } @@ -791,12 +780,12 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif .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_collision_steps,settle_max_penetration_m,settle_max_clearance_violation_m,settle_worst_contact_distance_m,settle_worst_contact,path_collision_steps,path_max_penetration_m,path_max_clearance_violation_m,path_worst_contact_distance_m,path_worst_contact,hold_collision_steps,hold_max_penetration_m,hold_max_clearance_violation_m,hold_worst_contact_distance_m,hold_worst_contact,collision_steps,max_penetration_m,max_clearance_violation_m,pass\n", + "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},{},{},{},{:.8},{:.8},{},{},{},{:.8},{:.8},{}", + "{},{},{},{},{},{},{},{},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{:.8},{},{:.8},{},{},{},{:.8},{},{},{},{:.8},{},{},{},{:.8},{}", row.scene, row.scene_file, row.query, @@ -814,22 +803,18 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif row.saturated_step_fraction, row.settle_collisions.steps, row.settle_collisions.max_penetration_m, - row.settle_collisions.max_clearance_violation_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, - row.path_collisions.max_clearance_violation_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, - row.hold_collisions.max_clearance_violation_m, format_optional_distance(row.hold_collisions.worst_contact_distance_m), row.hold_collisions.worst_contact.as_deref().unwrap_or(""), - row.collision_steps(), + row.penetration_steps(), row.max_penetration_m(), - row.max_clearance_violation_m(), row.pass ) .expect("format tracking CSV"); @@ -849,7 +834,7 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif "# 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 now passes only with **zero robot-collision steps** across settling, path execution, and hold under the repository's {:.3}-m clearance semantics. Signed contact distance, actual penetration, clearance violation, and worst geom pair are retained in the raw CSV.\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\ @@ -860,8 +845,7 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif PASS_RMS_RAD, PASS_MAX_RAD, HOLD_STEPS, - PASS_FINAL_RAD, - EXECUTION_CLEARANCE_M + PASS_FINAL_RAD ); for query in QUERIES { @@ -908,7 +892,7 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif report.push_str( "\n## Tracking results\n\n\ - Each cell is RMS / maximum / final six-joint L2 error in radians, followed by collision steps in settle/path/hold and maximum actual penetration. `PASS` requires all three numeric limits and zero collision steps.\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", ); @@ -935,7 +919,7 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif .expect("velocity row"); writeln!( report, - "| {} | {} | {} | {:.4}/{:.4}/{:.4}; c {}/{}/{}; pen {:.3} mm {} | {:.4}/{:.4}/{:.4}; c {}/{}/{}; pen {:.3} mm {} |", + "| {} | {} | {} | {:.4}/{:.4}/{:.4}; p {}/{}/{}; pen {:.3} mm {} | {:.4}/{:.4}/{:.4}; p {}/{}/{}; pen {:.3} mm {} |", scene.name, query.name, plant.name, @@ -976,49 +960,43 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif .iter() .filter(|row| row.controller == Controller::VelocityFf.name() && numeric_gates_pass(row)) .count(); - let collision_cases: Vec<&TrackingRow> = tracking + let penetration_cases: Vec<&TrackingRow> = tracking .iter() - .filter(|row| row.collision_steps() > 0) + .filter(|row| row.penetration_steps() > 0) .collect(); - let total_collision_steps: usize = collision_cases + let total_penetration_steps: usize = penetration_cases .iter() - .map(|row| row.collision_steps()) + .map(|row| row.penetration_steps()) .sum(); - let max_penetration_m = collision_cases + let max_penetration_m = penetration_cases .iter() .map(|row| row.max_penetration_m()) .fold(0.0f64, f64::max); - let max_clearance_violation_m = collision_cases - .iter() - .map(|row| row.max_clearance_violation_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-collision requirement.\n- PD + velocity feedforward: {velocity_numeric_passes}/{} meet the numeric tracking gates; {velocity_passes}/{} pass after the zero-collision requirement.\n- Executed-contact audit: {}/{} cases have robot collision steps; {total_collision_steps} total phase-steps, maximum actual penetration {:.6} m, maximum {:.3}-m-clearance violation {:.6} m.\n", + "\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, - collision_cases.len(), + penetration_cases.len(), tracking.len(), - max_penetration_m, - EXECUTION_CLEARANCE_M, - max_clearance_violation_m + max_penetration_m ) .expect("format aggregate result"); - if !collision_cases.is_empty() { + if !penetration_cases.is_empty() { report.push_str( - "\n## Executed collision cases\n\n\ - Collision steps are shown as settle/path/hold. The worst contact is selected by maximum clearance violation; signed distance below zero is actual penetration.\n\n\ - | Scene | Query | Plant | Controller | Steps S/P/H | Max penetration (m) | Max clearance violation (m) | Worst phase / signed distance / geom pair |\n\ - |---|---|---|---|---:|---:|---:|---|\n", + "\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 &collision_cases { + for row in &penetration_cases { let (phase, worst) = worst_collision(row); writeln!( report, - "| {} | {} | {} | {} | {}/{}/{} | {:.8} | {:.8} | {} / {:.8} / {} |", + "| {} | {} | {} | {} | {}/{}/{} | {:.8} | {} / {:.8} / {} |", row.scene, row.query, row.plant, @@ -1027,7 +1005,6 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif row.path_collisions.steps, row.hold_collisions.steps, row.max_penetration_m(), - row.max_clearance_violation_m(), phase, worst .worst_contact_distance_m @@ -1043,7 +1020,7 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif writeln!( report, - "\nThe fixed `tabletop_pillar/reverse_cross_workspace` fixture is retained in full, including any collision-negative outcomes; no fixture or failed case is removed from either artifact." + "\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"); @@ -1054,14 +1031,15 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif - 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. Executed collision checks sample each 2-ms simulation state in settle, path, and hold; neither is a continuous swept-volume certificate. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk.\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 collision counts/depths, and worst contact identities).\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 { @@ -1207,16 +1185,15 @@ mod tests { } #[test] - fn collision_violation_is_penetration_plus_clearance() { + fn execution_audit_uses_zero_threshold_and_actual_penetration() { let signed_distance: f64 = -0.004; let penetration = (-signed_distance).max(0.0); - let violation = (EXECUTION_CLEARANCE_M - signed_distance).max(0.0); + assert_eq!(EXECUTION_CONTACT_THRESHOLD_M, 0.0); assert!((penetration - 0.004).abs() < 1e-12); - assert!((violation - 0.005).abs() < 1e-12); } #[test] - fn collision_step_invalidates_otherwise_passing_case() { + 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)); } diff --git a/crates/arm-lab/src/collision.rs b/crates/arm-lab/src/collision.rs index bbe84ab..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,11 +23,11 @@ use mujoco_rs::prelude::*; use crate::chain::Chain; -/// A robot-involved MuJoCo contact below the checker's clearance threshold. +/// A robot-involved emitted MuJoCo contact below the checker's threshold. /// /// `distance_m` is MuJoCo's signed contact distance: negative values are -/// penetration, while positive values below [`CollisionChecker::clearance`] -/// violate the requested safety margin without geometric penetration. +/// 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, @@ -54,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 { @@ -89,7 +99,7 @@ impl> CollisionChecker { data, qpos_adr, robot_geom, - clearance: 1e-3, + contact_threshold: 1e-3, } } @@ -110,11 +120,11 @@ impl> CollisionChecker { pub fn collides(&mut self, q: &[f64]) -> bool { self.update_contacts(q); self.data.contact().iter().any(|contact| { - contact.dist < self.clearance && self.contact_involves_robot(contact.geom) + contact.dist < self.contact_threshold && self.contact_involves_robot(contact.geom) }) } - /// Robot-involved contacts whose signed distance is below `clearance`. + /// 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 @@ -126,7 +136,7 @@ impl> CollisionChecker { .contact() .iter() .filter(|contact| { - contact.dist < self.clearance && self.contact_involves_robot(contact.geom) + contact.dist < self.contact_threshold && self.contact_involves_robot(contact.geom) }) .map(|contact| RobotContact { geom1_id: contact.geom[0], diff --git a/crates/arm-lab/tests/pickplace.rs b/crates/arm-lab/tests/pickplace.rs index 9c595f3..dd30d91 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(); @@ -74,7 +75,7 @@ fn home_is_free_and_poses_are_reachable() { 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 ea5dc93..33a7768 100644 --- a/crates/arm-lab/tests/plan.rs +++ b/crates/arm-lab/tests/plan.rs @@ -182,6 +182,7 @@ fn collision_audit_matches_boolean_and_identifies_pillar() { 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; @@ -194,7 +195,7 @@ fn collision_audit_matches_boolean_and_identifies_pillar() { } let contacts = first_contacts.expect("pan sweep never contacted pillar"); assert!(contacts.iter().all(|contact| { - contact.distance_m < checker.clearance + contact.distance_m < 0.0 && !contact.geom1_name.is_empty() && !contact.geom2_name.is_empty() && !contact.body1_name.is_empty() From e6c4bd9c8d18edddec16b8562387638bd206f444 Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 02:30:05 +0100 Subject: [PATCH 6/7] Regenerate penetration-aware benchmark evidence --- README.md | 2 +- docs/multi_query_planning.csv | 90 +++++++++++++++++------------------ docs/multi_query_results.md | 69 ++++++++++++++------------- docs/multi_query_tracking.csv | 74 ++++++++++++++-------------- 4 files changed, 118 insertions(+), 117 deletions(-) diff --git a/README.md b/README.md index 579cc28..2821142 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ The generated [full results](docs/robustness_results.md) and [raw CSV](docs/robu `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. -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. +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 diff --git a/docs/multi_query_planning.csv b/docs/multi_query_planning.csv index 6300960..09815cf 100644 --- a/docs/multi_query_planning.csv +++ b/docs/multi_query_planning.csv @@ -1,46 +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.12429000,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.12327000,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.12346000,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.12200000,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.12163000,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.09689000,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.09466000,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.09391900,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.09342000,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.09335000,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.12424000,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.12570900,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.12300000,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.12283900,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.12298000,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.93587900,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,5.93811100,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,4.98799300,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.43564800,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.75752900,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.12946000,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.12333900,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.12260000,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.12263900,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.12287000,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.10958900,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.11385000,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.10687000,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.10672000,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.10679000,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.37385800,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.14670700,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.94178400,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.10870000,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.37058300,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.07293200,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,5.77112000,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.42939100,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.75411000,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.38588000,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.13495900,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.13814000,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.12981900,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.12978000,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.13728000,0,2,2,23,1.05830052,903,1.80328283 +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 index 317b2bf..2d1e0bd 100644 --- a/docs/multi_query_results.md +++ b/docs/multi_query_results.md @@ -4,7 +4,7 @@ This deterministic extension evaluates **nine fixed scene-query fixtures (three 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 now passes only with **zero robot-collision steps** across settling, path execution, and hold under the repository's 0.001-m clearance semantics. Signed contact distance, actual penetration, clearance violation, and worst geom pair are retained in the raw CSV. +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. @@ -24,50 +24,50 @@ The numeric tracking limits are reused unchanged from the earlier robustness env ## Tracking results -Each cell is RMS / maximum / final six-joint L2 error in radians, followed by collision steps in settle/path/hold and maximum actual penetration. `PASS` requires all three numeric limits and zero collision steps. +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; c 0/0/0; pen 0.000 mm FAIL | 0.0117/0.0118/0.0116; c 0/0/0; pen 0.000 mm PASS | -| open_floor | positive_pan | combined moderate shift | 0.0973/0.1163/0.0195; c 0/0/0; pen 0.000 mm FAIL | 0.0195/0.0202/0.0193; c 0/0/0; pen 0.000 mm PASS | -| open_floor | shoulder_elbow | nominal | 0.1117/0.1552/0.0173; c 0/0/0; pen 0.000 mm FAIL | 0.0140/0.0176/0.0176; c 0/0/0; pen 0.000 mm PASS | -| open_floor | shoulder_elbow | combined moderate shift | 0.1188/0.1643/0.0266; c 0/0/0; pen 0.000 mm FAIL | 0.0222/0.0270/0.0271; c 0/0/0; pen 0.000 mm FAIL | -| open_floor | wrist_reorientation | nominal | 0.1245/0.1640/0.0105; c 0/0/0; pen 0.000 mm FAIL | 0.0109/0.0116/0.0098; c 0/0/0; pen 0.000 mm PASS | -| open_floor | wrist_reorientation | combined moderate shift | 0.1339/0.1760/0.0187; c 0/0/0; pen 0.000 mm FAIL | 0.0200/0.0215/0.0176; c 0/0/0; pen 0.000 mm PASS | -| offset_pillar | positive_pan | nominal | 0.1409/0.1719/0.0111; c 0/0/0; pen 0.000 mm FAIL | 0.0097/0.0121/0.0117; c 0/0/0; pen 0.000 mm PASS | -| offset_pillar | positive_pan | combined moderate shift | 0.1498/0.1852/0.0187; c 0/0/0; pen 0.000 mm FAIL | 0.0186/0.0241/0.0194; c 0/0/0; pen 0.000 mm PASS | -| offset_pillar | negative_pan | nominal | 0.0897/0.1095/0.0120; c 0/0/0; pen 0.000 mm FAIL | 0.0117/0.0119/0.0117; c 0/0/0; pen 0.000 mm PASS | -| offset_pillar | negative_pan | combined moderate shift | 0.0957/0.1163/0.0196; c 0/0/0; pen 0.000 mm FAIL | 0.0195/0.0200/0.0193; c 0/0/0; pen 0.000 mm PASS | -| offset_pillar | shoulder_elbow | nominal | 0.1117/0.1552/0.0173; c 0/0/0; pen 0.000 mm FAIL | 0.0140/0.0176/0.0176; c 0/0/0; pen 0.000 mm PASS | -| offset_pillar | shoulder_elbow | combined moderate shift | 0.1188/0.1643/0.0266; c 0/0/0; pen 0.000 mm FAIL | 0.0222/0.0270/0.0271; c 0/0/0; pen 0.000 mm FAIL | -| tabletop_pillar | cross_workspace | nominal | 0.0998/0.1204/0.0152; c 0/0/0; pen 0.000 mm FAIL | 0.0132/0.0151/0.0152; c 0/0/0; pen 0.000 mm PASS | -| tabletop_pillar | cross_workspace | combined moderate shift | 0.1067/0.1278/0.0239; c 0/0/0; pen 0.000 mm FAIL | 0.0218/0.0252/0.0240; c 0/0/0; pen 0.000 mm FAIL | -| tabletop_pillar | reverse_cross_workspace | nominal | 0.1033/0.1213/0.0134; c 0/35/0; pen 0.055 mm FAIL | 0.0129/0.0143/0.0131; c 0/25/0; pen 0.050 mm FAIL | -| tabletop_pillar | reverse_cross_workspace | combined moderate shift | 0.1108/0.1315/0.0221; c 0/72/0; pen 0.079 mm FAIL | 0.0213/0.0241/0.0216; c 0/60/0; pen 0.079 mm FAIL | -| tabletop_pillar | wrist_reorientation | nominal | 0.1245/0.1640/0.0105; c 0/0/0; pen 0.000 mm FAIL | 0.0109/0.0116/0.0098; c 0/0/0; pen 0.000 mm PASS | -| tabletop_pillar | wrist_reorientation | combined moderate shift | 0.1339/0.1760/0.0187; c 0/0/0; pen 0.000 mm FAIL | 0.0200/0.0215/0.0176; c 0/0/0; pen 0.000 mm PASS | +| 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-collision requirement. -- PD + velocity feedforward: 14/18 meet the numeric tracking gates; 13/18 pass after the zero-collision requirement. -- Executed-contact audit: 4/36 cases have robot collision steps; 192 total phase-steps, maximum actual penetration 0.000079 m, maximum 0.001-m-clearance violation 0.001079 m. +- 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 collision cases +## Executed penetration cases -Collision steps are shown as settle/path/hold. The worst contact is selected by maximum clearance violation; signed distance below zero is actual penetration. +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) | Max clearance violation (m) | Worst phase / signed distance / geom pair | -|---|---|---|---|---:|---:|---:|---| -| tabletop_pillar | reverse_cross_workspace | nominal | position PD | 0/35/0 | 0.00005480 | 0.00105480 | 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 | 0.00105013 | 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 | 0.00107925 | 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 | 0.00107922 | path / -0.00007922 / geom#27[27]@wrist_2_link vs pillar[31]@pillar | +| 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 collision-negative outcomes; no fixture or failed case is removed from either artifact. +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 @@ -76,7 +76,8 @@ The fixed `tabletop_pillar/reverse_cross_workspace` fixture is retained in full, - 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. Executed collision checks sample each 2-ms simulation state in settle, path, and hold; neither is a continuous swept-volume certificate. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk. +- 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 @@ -86,4 +87,4 @@ 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 collision counts/depths, and worst contact identities). +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 index cb5d302..138208b 100644 --- a/docs/multi_query_tracking.csv +++ b/docs/multi_query_tracking.csv @@ -1,37 +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_collision_steps,settle_max_penetration_m,settle_max_clearance_violation_m,settle_worst_contact_distance_m,settle_worst_contact,path_collision_steps,path_max_penetration_m,path_max_clearance_violation_m,path_worst_contact_distance_m,path_worst_contact,hold_collision_steps,hold_max_penetration_m,hold_max_clearance_violation_m,hold_worst_contact_distance_m,hold_worst_contact,collision_steps,max_penetration_m,max_clearance_violation_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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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,0.00000000,,,35,0.00005480,0.00105480,-0.00005480,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,35,0.00005480,0.00105480,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,0.00000000,,,25,0.00005013,0.00105013,-0.00005013,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,25,0.00005013,0.00105013,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,0.00000000,,,72,0.00007925,0.00107925,-0.00007925,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,72,0.00007925,0.00107925,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,0.00000000,,,60,0.00007922,0.00107922,-0.00007922,geom#27[27]@wrist_2_link vs pillar[31]@pillar,0,0.00000000,0.00000000,,,60,0.00007922,0.00107922,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,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.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,,,0,0.00000000,0.00000000,true +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 From 2e4788a54f86b2c64423e2a18f5e14c9b2b0b415 Mon Sep 17 00:00:00 2001 From: Dylan Gallagher Date: Mon, 17 Aug 2026 02:37:09 +0100 Subject: [PATCH 7/7] Clarify pick-place cube collision scope --- assets/ur5e/scene_pickplace.xml | 10 +++------- crates/arm-lab/tests/pickplace.rs | 8 ++++++++ 2 files changed, 11 insertions(+), 7 deletions(-) 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 @@