diff --git a/README.md b/README.md index 2821142..b174f6e 100644 --- a/README.md +++ b/README.md @@ -4,9 +4,9 @@ A serial-chain manipulator stack written **from scratch in Rust** — no ROS, no ![Demo 3 — UR5e pick-and-place around a pillar. IK, RRT-Connect, scalar S-curve time law, PD + velocity feedforward](docs/demo3.gif) -The kinematic chain (offsets, joint axes, limits, end-effector site) is **extracted from the compiled MuJoCo model** — never hand-entered as DH parameters — so the geometry used by the algorithms is guaranteed identical to the one the physics simulates. Forward kinematics, geometric Jacobians, damped-least-squares inverse kinematics, RRT-Connect with shortcutting, and a rest-to-rest 7-phase scalar S-curve are implemented in this repo. Collision checks call `mj_collision` on interpolated joint-space states. The independent `k` + `urdf-rs` stack is used **only inside the test suite** as a cross-check of FK. +The kinematic chain (offsets, joint axes, limits, end-effector site) is **extracted from the compiled MuJoCo model** — never hand-entered as DH parameters — so the geometry used by the algorithms is guaranteed identical to the one the physics simulates. Forward kinematics, geometric Jacobians, damped-least-squares inverse kinematics, RRT-Connect with shortcutting, and a rest-to-rest 7-phase scalar S-curve are implemented in this repo. Robot collision checks call `mj_collision` on interpolated joint-space states; Demo 3 additionally uses explicit, pair-scoped `mj_geomDistance` queries for an EE-attached cube proxy. The independent `k` + `urdf-rs` stack is used **only inside the test suite** as a cross-check of FK. -Demo 3 (above): IK solves pick and place poses; the joint-space straight line between them hits a pillar on the table; RRT-Connect carries the cube around it in **~4 ms**; the path coordinate is timed with an S-curve and tracked to **0.004 rad**. Polyline corners are not blended, so the joint trajectory is not globally jerk-limited. The cube is a mocap weld (scripted attach, not contact-rich grasping). Demo 2 (pillar dodge, no object) is in [`docs/demo2.gif`](docs/demo2.gif). Demo 1 (IK target sequence) is in [`docs/demo1.gif`](docs/demo1.gif). +Demo 3 (above): IK solves pick and place poses; the joint-space straight line between them violates both the robot predicate and the attached cube's 5 mm pillar margin. Fixed-seed RRT-Connect finds a 6-waypoint sampled-clear carry in **5.4–7.8 ms** across two repeated headless runs; the path coordinate is timed with an S-curve and tracked to **0.0067 rad** worst error. Polyline corners are not blended, so the joint trajectory is not globally jerk-limited. The cube is a mocap weld (scripted attach, not contact-rich grasping). Demo 2 (pillar dodge, no object) is in [`docs/demo2.gif`](docs/demo2.gif). Demo 1 (IK target sequence) is in [`docs/demo1.gif`](docs/demo1.gif). ## Numbers (measured, reproducible via `cargo test`) @@ -24,8 +24,9 @@ Demo 3 (above): IK solves pick and place poses; the joint-space straight line be | Demo 2 scalar S-curve time law (seed `20260816`, v≤0.55 a≤1.8 j≤8) | **1751 samples, 3.50 s**, peak joint \|qd\| 0.550 rad/s | | Demo 2 tracking (PD + vel FF + gravity compensation) | worst 0.0046 rad · final goal 0.0002 rad | | Timed-trajectory determinism | identical `q(t)` given identical seed + limits | -| Demo 3 carry (pick→place around table pillar, seed `20260816`) | **~4 ms**, 4 waypoints after shortcut | -| Demo 3 tracking (full pick-and-place) | worst 0.0037 rad | +| Demo 3 attached-cube carry (pick→place, seed `20260816`) | **6 waypoints, 47 sampled states**, 0/47 payload-margin violations | +| Demo 3 sampled minimum cube/environment distance (5 mm threshold) | **11.849 mm** vs pillar | +| Demo 3 tracking (full pick-and-place, representative headless run) | worst 0.0067 rad | The 1 s median planning-time exit criterion is met by two orders of magnitude. The IK stress test samples targets from valid random joint configurations; the 2.2% non-converged slice is reported without assigning an untested cause. The solver honors limits by construction (clamped every iteration, asserted in tests). @@ -35,6 +36,12 @@ The 1 s median planning-time exit criterion is met by two orders of magnitude. T The generated [full results](docs/robustness_results.md) and [raw CSV](docs/robustness_results.csv) state their pass thresholds and limitations. This is a simulation stress test, not hardware validation or a sim-to-real guarantee. +## Attached-payload carry check (simulation) + +Demo 3 now transforms the compiled 50 mm cube geom from end-effector FK at every carry query and calls MuJoCo's explicit pair-distance API only for cube↔floor, cube↔table, and cube↔pillar. A state is rejected below a declared 5 mm payload margin, while the original robot rule remains a zero-penetration check. Both predicates independently block the 23-sample straight edge. The fixed-seed plan succeeds with 6 shortcut waypoints and 47 densified states; a fresh checker finds zero robot collisions, zero payload-margin violations, and an 11.849 mm minimum sampled cube distance. + +The [pre-result protocol](docs/attached_payload_protocol.md) and [result/limitations report](docs/attached_payload_results.md) preserve the exact pair scope, thresholds, negative outcomes, transform regression, and sampled-only claim boundary. This does not model object slip, grasp uncertainty, fingers, continuous swept volume, or payload-versus-robot self-contact. + ## Multi-scene, multi-query extension (simulation) `multi_query_bench` adds nine fixed scene-query fixtures (three per shipped MJCF scene, six unique joint-pair definitions) while retaining the original declared numeric tracking thresholds. Selected joint pairs are deliberately repeated across scenes to isolate geometry effects. Across five fixed planner seeds per fixture, all **30/30 direct-free** and **15/15 obstructed** trials succeeded. Executed states are checked after every settling, path, and hold step with a threshold of exactly 0.0 m; a case passes only with zero sampled robot contacts whose signed distance is negative. This is a sampled penetration gate, not a positive-clearance certificate. @@ -92,12 +99,13 @@ Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PAT - **Chain extraction, not re-modeling.** `Chain::from_mujoco` walks `body_parentid` from the tip body to the world, collecting static transforms, hinge axes, anchors, and limits from the *compiled* model, plus the EE site as the tool frame. One source of truth for geometry. - **DLS IK with adaptive damping.** Each step solves `Δq = Jᵀ(JJᵀ + λ²I)⁻¹e` with a diagonal nullspace bias toward a rest pose; λ scales down with the error so the endgame converges Newton-like while near-singular regions stay damped. Joint limits are clamped every iteration; seeded random restarts (TRAC-IK style) recover from bad basins. -- **RRT-Connect, from scratch.** Two trees grow toward each other (Kuffner & LaValle 2000). `EXTEND` takes one joint-space step; `CONNECT` greedily repeats it. Edges are collision-checked discretely by interpolating at `resolution` (0.05 rad L2 by default) and calling MuJoCo `mj_collision` on each state; this is sampled collision checking, not continuous certification. Greedy then random shortcutting removes redundant waypoints; the path is densified to the same resolution for execution. Sampling uses the in-repo SplitMix64 RNG — same seed, same path. +- **RRT-Connect, from scratch.** Two trees grow toward each other (Kuffner & LaValle 2000). `EXTEND` takes one joint-space step; `CONNECT` greedily repeats it. Edges are collision-checked discretely by interpolating at `resolution` (0.05 rad L2 by default); the robot predicate calls MuJoCo `mj_collision`, and Demo 3's carry also calls explicit distance queries for three payload/environment pairs. This is sampled collision checking, not continuous certification. Greedy then random shortcutting removes redundant waypoints; the path is densified to the same resolution for execution. Sampling uses the in-repo SplitMix64 RNG — same seed, same path. - **Collision filter.** Only contacts that involve a robot collision geom (`contype ≠ 0`, attached to a chain body, not the world) count. Floor-vs-pillar contacts are ignored; parent–child pairs are already excluded by MuJoCo. Visual meshes never participate. +- **Pair-scoped attached load.** `AttachedBoxCollisionChecker` solves the mocap-body pose so the compiled proxy geom equals `T_world_EE * T_EE_proxy`, including nonzero geom-local offsets. It queries only constructor-validated environment names and rejects signed distances `< 0.005 m` in Demo 3. Robot and place-pad geoms are excluded from this scope; no global positive robot threshold is applied. - **Deterministic.** Restart sampling, RRT sampling, and random shortcutting all use the in-repo RNG with a fixed seed. The timed trajectory is bit-stable given the same seed and limits (CI golden test). - **Scalar S-curve time law.** A rest-to-rest 7-phase bang-bang-jerk profile times the scalar path length `s ∈ [0, L]`. Per-joint `(v, a, j)` limits are converted to path-space limits by the steepest `|dqᵢ/ds|` on the polyline, so no joint exceeds its bound within an edge. Polyline tangent discontinuities are not blended: joint velocity can jump at a corner, so the complete joint trajectory is not globally acceleration- or jerk-bounded. -- **Physics-side servo.** The demos apply the exact MuJoCo bias force as gravity/Coriolis feedforward. Position actuators are commanded as a PD tracker with velocity feedforward: `ctrl = q_des + (kv/kp)·qd_des` yields `τ = kp(q_des − q) + kv(qd_des − qd)`. Worst joint-space tracking is 0.0046 rad (Demo 2) and 0.0037 rad (Demo 3). -- **Scripted grasp.** Demo 3 welds a mocap cube to the EE after the pick descend and parks it on the place pad after the place descend. That is a kinematics/planning demo, not contact-rich grasping. The cube volume is not represented in the planner collision geometry, and carry planning uses a zero contact threshold that only rejects sampled robot penetration. Attached-load and pair-scoped clearance checks remain future work. +- **Physics-side servo.** The demos apply the exact MuJoCo bias force as gravity/Coriolis feedforward. Position actuators are commanded as a PD tracker with velocity feedforward: `ctrl = q_des + (kv/kp)·qd_des` yields `τ = kp(q_des − q) + kv(qd_des − qd)`. Worst joint-space tracking is 0.0046 rad (Demo 2) and 0.0067 rad in the representative attached-load Demo 3 run. +- **Scripted grasp.** Demo 3 welds a mocap cube to the EE after the pick descend and parks it on the place pad after the place descend. During carry, that same compiled box is a 5 mm pair-scoped planning proxy against floor, table, and pillar. This remains a kinematics/planning demo, not contact-rich grasping: it does not model slip, fingers, compliance, grasp uncertainty, continuous swept volume, calibration error, or cube-versus-robot self-contact. ## Roadmap @@ -107,6 +115,7 @@ Requirements: Rust stable, a C++ toolchain, and (for `--render`) `ffmpeg` on PAT - [x] Pick-and-place with obstacle dodging; benchmark tables - [x] Reproducible controller robustness matrix with raw CSV and explicit sim-only limits - [x] Multi-scene, multi-query planning and tracking extension with raw CSVs +- [x] Pair-scoped attached-cube collision proxy for Demo 3 carry ## License & assets diff --git a/crates/arm-lab-demo/src/bin/demo3.rs b/crates/arm-lab-demo/src/bin/demo3.rs index 6838192..d9576bf 100644 --- a/crates/arm-lab-demo/src/bin/demo3.rs +++ b/crates/arm-lab-demo/src/bin/demo3.rs @@ -1,9 +1,10 @@ //! Demo 3 — pick-and-place around a pillar, from scratch. //! //! IK solves grasp poses, RRT-Connect carries the cube around a pillar that -//! blocks the joint-space interpolant, a jerk-bounded scalar S-curve times every -//! segment, and a mocap weld stands in for a gripper (scripted attach, not -//! contact-rich grasping). +//! blocks the joint-space interpolant, a pair-scoped attached-box proxy checks +//! the carried cube against the environment, a jerk-bounded scalar S-curve +//! times every segment, and a mocap weld stands in for a gripper (scripted +//! attach, not contact-rich grasping). //! //! ```text //! cargo run --release -p arm-lab-demo --bin demo3 @@ -16,7 +17,9 @@ use arm_lab::ik::{IkConfig, solve_ik}; use arm_lab::kinematics::fk; use arm_lab::plan::rrt_connect; use arm_lab::traj::{TrajLimits, time_parameterize}; -use arm_lab::{Chain, CollisionChecker, PlanConfig, PlanStatus}; +use arm_lab::{ + AttachedBoxCollisionChecker, AttachedBoxSpec, Chain, CollisionChecker, PlanConfig, PlanStatus, +}; use arm_lab_demo::{ GIF_FPS, RENDER_EVERY, RENDER_H, RENDER_W, capture_frame, encode_gif, gravity_compensate, init_recording, log_transform, parse_args, read_q, set_ctrl, traj_step, @@ -53,10 +56,10 @@ const KV_OVER_KP: f64 = 0.2; const LOG_EVERY: usize = 2; const SETTLE_STEPS: usize = 80; /// Carry planning rejects sampled robot penetration. -/// -/// The carried cube is not represented in the planner collision geometry, so -/// this does not certify clearance for the attached load. const CARRY_CONTACT_THRESHOLD: f64 = 0.0; +/// Pair-scoped positive planning buffer for the attached cube only. +const PAYLOAD_CLEARANCE_M: f64 = 0.005; +const PAYLOAD_ENVIRONMENT: [&str; 3] = ["floor", "table", "pillar"]; const SEED: u64 = 20260816; fn main() { @@ -86,6 +89,18 @@ fn main() { }; let mut cc = CollisionChecker::new(&model, &chain); assert!(!cc.collides(&q_home), "home is in collision"); + let mut carry_cc = AttachedBoxCollisionChecker::new( + &model, + &chain, + AttachedBoxSpec::new( + "cube", + Isometry3::translation(CUBE_IN_EE.x, CUBE_IN_EE.y, CUBE_IN_EE.z), + PAYLOAD_ENVIRONMENT, + PAYLOAD_CLEARANCE_M, + ), + ) + .expect("valid attached cube collision proxy"); + carry_cc.set_robot_contact_threshold(CARRY_CONTACT_THRESHOLD); let q_pick_app = solve_named( "pick_approach", @@ -216,14 +231,13 @@ fn main() { &q_pick_app, 1e-3, ); - q = go( + q = go_with_collision( "carry around pillar", &mut ctx, - &mut cc, &plan_cfg, &q, &q_place_app, - CARRY_CONTACT_THRESHOLD, + &mut |candidate| carry_cc.collides(candidate), ); q = go( "descend to place", @@ -300,8 +314,19 @@ fn go( ) -> Vec { cc.contact_threshold = contact_threshold; let mut collides = |q: &[f64]| cc.collides(q); + go_with_collision(name, ctx, plan_cfg, q_from, q_to, &mut collides) +} + +fn go_with_collision( + name: &str, + ctx: &mut ExecCtx<'_, '_>, + plan_cfg: &PlanConfig, + q_from: &[f64], + q_to: &[f64], + collides: &mut impl FnMut(&[f64]) -> bool, +) -> Vec { let t0 = std::time::Instant::now(); - let plan = rrt_connect(ctx.chain, q_from, q_to, &mut collides, plan_cfg); + let plan = rrt_connect(ctx.chain, q_from, q_to, collides, plan_cfg); let plan_ms = t0.elapsed().as_secs_f64() * 1e3; assert_eq!( plan.status, diff --git a/crates/arm-lab/src/collision.rs b/crates/arm-lab/src/collision.rs index be6bc29..5d3e4d9 100644 --- a/crates/arm-lab/src/collision.rs +++ b/crates/arm-lab/src/collision.rs @@ -16,12 +16,22 @@ //! 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`). +//! +//! [`AttachedBoxCollisionChecker`] adds a separate mechanism for a rigid +//! end-effector load: it transforms a compiled mocap box from FK and calls +//! MuJoCo's explicit geom-distance function only for constructor-declared +//! payload/environment pairs. A positive payload clearance therefore does not +//! inflate or otherwise change the robot contact predicate. +use std::collections::HashSet; +use std::fmt; use std::ops::Deref; use mujoco_rs::prelude::*; +use nalgebra::{Isometry3, Matrix3, Quaternion, Rotation3, Translation3, UnitQuaternion}; use crate::chain::Chain; +use crate::kinematics::fk; /// A robot-involved emitted MuJoCo contact below the checker's threshold. /// @@ -54,6 +64,138 @@ impl RobotContact { } } +/// Declarative geometry for an end-effector-attached box proxy. +/// +/// The named proxy must already exist as a box geom on a MuJoCo mocap body. +/// Its contact masks may be disabled: [`AttachedBoxCollisionChecker`] uses +/// explicit pair-distance queries, not the emitted-contact set, for payload +/// proximity. `proxy_in_ee` is the desired proxy-*geom* pose in the extracted +/// end-effector frame, not the mocap-body pose. +#[derive(Debug, Clone)] +pub struct AttachedBoxSpec { + pub proxy_geom_name: String, + pub proxy_in_ee: Isometry3, + pub environment_geom_names: Vec, + pub clearance_m: f64, +} + +impl AttachedBoxSpec { + pub fn new( + proxy_geom_name: impl Into, + proxy_in_ee: Isometry3, + environment_geom_names: I, + clearance_m: f64, + ) -> Self + where + I: IntoIterator, + S: Into, + { + Self { + proxy_geom_name: proxy_geom_name.into(), + proxy_in_ee, + environment_geom_names: environment_geom_names.into_iter().map(Into::into).collect(), + clearance_m, + } + } +} + +/// A pair-scoped distance produced by [`AttachedBoxCollisionChecker`]. +/// +/// Distances below `clearance_m` are exact for the collision decision. Free +/// distances can be capped at the checker's finite query bound (at least 1 m), +/// because MuJoCo's pair query accepts a maximum distance. +#[derive(Debug, Clone, PartialEq)] +pub struct PayloadPairDistance { + pub proxy_geom_id: usize, + pub proxy_geom_name: String, + pub environment_geom_id: usize, + pub environment_geom_name: String, + pub distance_m: f64, + pub clearance_m: f64, +} + +impl PayloadPairDistance { + /// The strict predicate used by the planner. + pub fn violates_clearance(&self) -> bool { + !self.distance_m.is_finite() || self.distance_m < self.clearance_m + } + + /// Stable human-readable pair identity for diagnostics. + pub fn identity(&self) -> String { + format!( + "{}[{}] vs {}[{}]", + self.proxy_geom_name, + self.proxy_geom_id, + self.environment_geom_name, + self.environment_geom_id + ) + } +} + +/// Construction failures for [`AttachedBoxCollisionChecker`]. +#[derive(Debug, Clone, PartialEq)] +pub enum AttachedBoxError { + EmptyEnvironmentSet, + UnknownProxyGeom(String), + ProxyIsNotBox { name: String, geom_type: MjtGeom }, + ProxyBodyIsNotMocap { geom: String, body: String }, + UnknownEnvironmentGeom(String), + DuplicateEnvironmentGeom(String), + ProxyInEnvironmentSet(String), + EnvironmentGeomIsNotContactEnabled(String), + EnvironmentGeomBelongsToRobot(String), + NonFiniteProxyTransform, + InvalidClearance(f64), +} + +impl fmt::Display for AttachedBoxError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::EmptyEnvironmentSet => write!(f, "payload environment set is empty"), + Self::UnknownProxyGeom(name) => write!(f, "unknown proxy geom '{name}'"), + Self::ProxyIsNotBox { name, geom_type } => { + write!(f, "proxy geom '{name}' is {geom_type:?}, not a box") + } + Self::ProxyBodyIsNotMocap { geom, body } => { + write!(f, "proxy geom '{geom}' belongs to non-mocap body '{body}'") + } + Self::UnknownEnvironmentGeom(name) => { + write!(f, "unknown environment geom '{name}'") + } + Self::DuplicateEnvironmentGeom(name) => { + write!(f, "duplicate environment geom '{name}'") + } + Self::ProxyInEnvironmentSet(name) => { + write!(f, "proxy geom '{name}' is also in the environment set") + } + Self::EnvironmentGeomIsNotContactEnabled(name) => write!( + f, + "environment geom '{name}' has both contact masks disabled" + ), + Self::EnvironmentGeomBelongsToRobot(name) => { + write!(f, "environment geom '{name}' belongs to the robot chain") + } + Self::NonFiniteProxyTransform => { + write!(f, "proxy transform contains a non-finite component") + } + Self::InvalidClearance(clearance) => { + write!( + f, + "payload clearance must be finite and non-negative, got {clearance}" + ) + } + } + } +} + +impl std::error::Error for AttachedBoxError {} + +#[derive(Debug, Clone)] +struct ScopedEnvironmentGeom { + id: usize, + name: String, +} + /// MuJoCo-backed collision oracle for a serial chain. pub struct CollisionChecker> { data: MjData, @@ -172,6 +314,288 @@ impl> CollisionChecker { } } +/// Pair-scoped collision oracle for a rigid box attached to the end effector. +/// +/// This checker combines two deliberately different predicates at each sampled +/// joint state: +/// +/// 1. the existing [`CollisionChecker`] emitted-contact rule for the robot, and +/// 2. explicit MuJoCo geom-distance queries from one attached proxy box to a +/// constructor-validated list of environment geoms. +/// +/// It does **not** globally inflate the robot, and it does not query the proxy +/// against robot geoms. That latter exclusion is intentional for a scripted +/// grasp whose allowed wrist/load contact region is not modeled. +pub struct AttachedBoxCollisionChecker> { + robot: CollisionChecker, + chain: Chain, + proxy_geom_id: usize, + proxy_geom_name: String, + proxy_mocap_id: usize, + /// Local geom pose relative to its mocap body. The mocap pose is solved so + /// the *geom*, rather than merely its body origin, matches `proxy_in_ee`. + proxy_geom_in_body: Isometry3, + proxy_in_ee: Isometry3, + environment: Vec, + clearance_m: f64, + distance_query_bound_m: f64, +} + +impl> AttachedBoxCollisionChecker { + /// Construct and validate a pair-scoped attached-box checker. + pub fn new(model: M, chain: &Chain, spec: AttachedBoxSpec) -> Result { + if !spec.clearance_m.is_finite() || spec.clearance_m < 0.0 { + return Err(AttachedBoxError::InvalidClearance(spec.clearance_m)); + } + if !is_finite_isometry(&spec.proxy_in_ee) { + return Err(AttachedBoxError::NonFiniteProxyTransform); + } + if spec.environment_geom_names.is_empty() { + return Err(AttachedBoxError::EmptyEnvironmentSet); + } + + let robot = CollisionChecker::new(model, chain); + let compiled = robot.data.model(); + let proxy_geom_id = compiled + .name_to_id(MjtObj::mjOBJ_GEOM, &spec.proxy_geom_name) + .ok_or_else(|| AttachedBoxError::UnknownProxyGeom(spec.proxy_geom_name.clone()))?; + let proxy_type = compiled.geom_type()[proxy_geom_id]; + if proxy_type != MjtGeom::mjGEOM_BOX { + return Err(AttachedBoxError::ProxyIsNotBox { + name: spec.proxy_geom_name, + geom_type: proxy_type, + }); + } + + let proxy_body_id = compiled.geom_bodyid()[proxy_geom_id] as usize; + let proxy_mocap_id = compiled.body_mocapid()[proxy_body_id]; + if proxy_mocap_id < 0 { + return Err(AttachedBoxError::ProxyBodyIsNotMocap { + geom: spec.proxy_geom_name, + body: body_name(compiled, proxy_body_id), + }); + } + + let proxy_geom_in_body = geom_local_pose(compiled, proxy_geom_id); + if !is_finite_isometry(&proxy_geom_in_body) { + return Err(AttachedBoxError::NonFiniteProxyTransform); + } + + let mut robot_body = vec![false; compiled.body_parentid().len()]; + for link in chain.links() { + if link.body_id != 0 { + robot_body[link.body_id] = true; + } + } + + let mut seen = HashSet::new(); + let mut environment = Vec::with_capacity(spec.environment_geom_names.len()); + for name in spec.environment_geom_names { + let id = compiled + .name_to_id(MjtObj::mjOBJ_GEOM, &name) + .ok_or_else(|| AttachedBoxError::UnknownEnvironmentGeom(name.clone()))?; + if id == proxy_geom_id { + return Err(AttachedBoxError::ProxyInEnvironmentSet(name)); + } + if !seen.insert(id) { + return Err(AttachedBoxError::DuplicateEnvironmentGeom(name)); + } + if compiled.geom_contype()[id] == 0 && compiled.geom_conaffinity()[id] == 0 { + return Err(AttachedBoxError::EnvironmentGeomIsNotContactEnabled(name)); + } + let body_id = compiled.geom_bodyid()[id] as usize; + if robot_body[body_id] { + return Err(AttachedBoxError::EnvironmentGeomBelongsToRobot(name)); + } + environment.push(ScopedEnvironmentGeom { id, name }); + } + + Ok(Self { + robot, + chain: chain.clone(), + proxy_geom_id, + proxy_geom_name: spec.proxy_geom_name, + proxy_mocap_id: proxy_mocap_id as usize, + proxy_geom_in_body, + proxy_in_ee: spec.proxy_in_ee, + environment, + clearance_m: spec.clearance_m, + // Distances beyond the decision threshold are not needed. A 1 m + // lower bound keeps ordinary free-state diagnostics informative + // while retaining a finite MuJoCo `distmax`. + distance_query_bound_m: spec.clearance_m.max(1.0), + }) + } + + /// Set the unchanged robot emitted-contact threshold used in the combined + /// predicate. Demo 3 carry uses exactly `0.0 m` (sampled penetration). + pub fn set_robot_contact_threshold(&mut self, threshold_m: f64) { + self.robot.contact_threshold = threshold_m; + } + + pub fn robot_contact_threshold(&self) -> f64 { + self.robot.contact_threshold + } + + pub fn payload_clearance_m(&self) -> f64 { + self.clearance_m + } + + pub fn proxy_geom_name(&self) -> &str { + &self.proxy_geom_name + } + + /// Actual compiled proxy-geom world pose after applying `q` and the + /// end-effector attachment transform. + /// + /// This reads MuJoCo's `geom_xpos`/`geom_xmat`, so tests can verify the + /// complete FK → mocap-body → nonzero geom-local transform pipeline rather + /// than inferring it from collision outcomes. + pub fn proxy_world_pose(&mut self, q: &[f64]) -> Isometry3 { + self.update_state(q); + geom_world_pose(&self.robot.data, self.proxy_geom_id) + } + + /// Environment geom names in the exact deterministic query order. + pub fn environment_geom_names(&self) -> impl Iterator { + self.environment.iter().map(|geom| geom.name.as_str()) + } + + /// True if the existing robot predicate rejects `q`. + pub fn robot_collides(&mut self, q: &[f64]) -> bool { + self.update_state(q); + self.robot_collides_current() + } + + /// All declared payload/environment pair distances at `q`. + pub fn payload_distances(&mut self, q: &[f64]) -> Vec { + self.update_state(q); + self.payload_distances_current() + } + + /// Only pair distances that violate the strict payload clearance rule. + pub fn payload_violations(&mut self, q: &[f64]) -> Vec { + self.payload_distances(q) + .into_iter() + .filter(PayloadPairDistance::violates_clearance) + .collect() + } + + /// True if any declared payload/environment pair is closer than the strict + /// payload clearance at `q`. + pub fn payload_collides(&mut self, q: &[f64]) -> bool { + self.update_state(q); + self.payload_collides_current() + } + + /// Combined sampled predicate: unchanged robot rule OR payload proximity. + pub fn collides(&mut self, q: &[f64]) -> bool { + self.update_state(q); + let robot_collision = self.robot_collides_current(); + let payload_collision = self.payload_collides_current(); + robot_collision || payload_collision + } + + fn update_state(&mut self, q: &[f64]) { + let desired_proxy_world = fk(&self.chain, q) * self.proxy_in_ee; + let mocap_body_world = desired_proxy_world * self.proxy_geom_in_body.inverse(); + let translation = mocap_body_world.translation.vector; + let rotation = mocap_body_world.rotation; + self.robot.data.mocap_pos_mut()[self.proxy_mocap_id] = + [translation.x, translation.y, translation.z]; + self.robot.data.mocap_quat_mut()[self.proxy_mocap_id] = + [rotation.w, rotation.i, rotation.j, rotation.k]; + self.robot.update_contacts(q); + } + + fn robot_collides_current(&self) -> bool { + self.robot.data.contact().iter().any(|contact| { + contact.dist < self.robot.contact_threshold + && self.robot.contact_involves_robot(contact.geom) + // The proxy may intentionally touch the wrist/load attachment + // region. That pair is outside the robot/environment rule and + // outside the explicitly scoped payload/environment queries. + && !contact.geom.contains(&(self.proxy_geom_id as i32)) + }) + } + + fn payload_distances_current(&mut self) -> Vec { + let mut distances = Vec::with_capacity(self.environment.len()); + for index in 0..self.environment.len() { + let environment_geom_id = self.environment[index].id; + let environment_geom_name = self.environment[index].name.clone(); + let distance_m = self.robot.data.geom_distance( + self.proxy_geom_id, + environment_geom_id, + self.distance_query_bound_m, + None, + ); + distances.push(PayloadPairDistance { + proxy_geom_id: self.proxy_geom_id, + proxy_geom_name: self.proxy_geom_name.clone(), + environment_geom_id, + environment_geom_name, + distance_m, + clearance_m: self.clearance_m, + }); + } + distances + } + + fn payload_collides_current(&mut self) -> bool { + for index in 0..self.environment.len() { + let distance_m = self.robot.data.geom_distance( + self.proxy_geom_id, + self.environment[index].id, + self.distance_query_bound_m, + None, + ); + if !distance_m.is_finite() || distance_m < self.clearance_m { + return true; + } + } + false + } +} + +fn geom_local_pose(model: &MjModel, geom_id: usize) -> Isometry3 { + let position = model.geom_pos()[geom_id]; + let quaternion = model.geom_quat()[geom_id]; + Isometry3::from_parts( + Translation3::new(position[0], position[1], position[2]), + UnitQuaternion::new_normalize(Quaternion::new( + quaternion[0], + quaternion[1], + quaternion[2], + quaternion[3], + )), + ) +} + +fn geom_world_pose>(data: &MjData, geom_id: usize) -> Isometry3 { + let position = data.geom_xpos()[geom_id]; + let matrix = Matrix3::from_row_slice(&data.geom_xmat()[geom_id]); + Isometry3::from_parts( + Translation3::new(position[0], position[1], position[2]), + UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked(matrix)), + ) +} + +fn is_finite_isometry(pose: &Isometry3) -> bool { + pose.translation + .vector + .iter() + .all(|value| value.is_finite()) + && pose.rotation.coords.iter().all(|value| value.is_finite()) +} + +fn body_name(model: &MjModel, id: usize) -> String { + model + .id_to_name(MjtObj::mjOBJ_BODY, id) + .filter(|name| !name.is_empty()) + .map_or_else(|| format!("body#{id}"), str::to_string) +} + fn geom_name(model: &MjModel, id: i32) -> String { if id < 0 { return "none".to_string(); diff --git a/crates/arm-lab/src/lib.rs b/crates/arm-lab/src/lib.rs index 562798f..ec80cd0 100644 --- a/crates/arm-lab/src/lib.rs +++ b/crates/arm-lab/src/lib.rs @@ -29,7 +29,10 @@ pub mod rng; pub mod traj; pub use chain::{Chain, Joint, Link}; -pub use collision::{CollisionChecker, RobotContact}; +pub use collision::{ + AttachedBoxCollisionChecker, AttachedBoxError, AttachedBoxSpec, CollisionChecker, + PayloadPairDistance, RobotContact, +}; pub use ik::{IkConfig, IkResult}; pub use plan::{PlanConfig, PlanResult, PlanStatus}; pub use rng::Rng; diff --git a/crates/arm-lab/tests/pickplace.rs b/crates/arm-lab/tests/pickplace.rs index 7815117..881c780 100644 --- a/crates/arm-lab/tests/pickplace.rs +++ b/crates/arm-lab/tests/pickplace.rs @@ -1,12 +1,17 @@ //! Pick-and-place scene: IK reachability, blocked carry interpolant, //! RRT-Connect around the pillar. +use std::collections::HashSet; + use arm_lab::ik::{IkConfig, solve_ik}; use arm_lab::kinematics::fk; -use arm_lab::plan::{edge_free, rrt_connect}; -use arm_lab::{Chain, CollisionChecker, PlanConfig, PlanStatus}; +use arm_lab::plan::{densify, edge_free, rrt_connect}; +use arm_lab::{ + AttachedBoxCollisionChecker, AttachedBoxError, AttachedBoxSpec, Chain, CollisionChecker, + PlanConfig, PlanStatus, +}; use mujoco_rs::prelude::*; -use nalgebra::{Isometry3, Translation3}; +use nalgebra::{Isometry3, Translation3, UnitQuaternion}; const SCENE: &str = concat!( env!("CARGO_MANIFEST_DIR"), @@ -20,6 +25,8 @@ const PLACE: [f64; 3] = [0.22, 0.58, 0.42]; const SEED: u64 = 20260816; // The cube is absent from planner geometry; this only rejects robot penetration. const CARRY_CONTACT_THRESHOLD: f64 = 0.0; +const PAYLOAD_CLEARANCE_M: f64 = 0.005; +const PAYLOAD_ENVIRONMENT: [&str; 3] = ["floor", "table", "pillar"]; fn load() -> (MjModel, Chain, Vec) { let model = MjModel::from_xml(SCENE).unwrap(); @@ -59,6 +66,24 @@ fn ik_at( ik.q } +fn payload_spec() -> AttachedBoxSpec { + AttachedBoxSpec::new( + "cube", + Isometry3::translation(0.0, 0.0, 0.035), + PAYLOAD_ENVIRONMENT, + PAYLOAD_CLEARANCE_M, + ) +} + +fn attached_checker<'a>( + model: &'a MjModel, + chain: &Chain, +) -> AttachedBoxCollisionChecker<&'a MjModel> { + let mut checker = AttachedBoxCollisionChecker::new(model, chain, payload_spec()).unwrap(); + checker.set_robot_contact_threshold(CARRY_CONTACT_THRESHOLD); + checker +} + #[test] fn home_is_free_and_poses_are_reachable() { let (model, chain, q_home) = load(); @@ -113,3 +138,376 @@ fn carry_straight_line_hits_pillar_rrt_succeeds() { plan.waypoints.len() ); } + +#[test] +fn attached_cube_endpoints_are_free_and_pair_scope_excludes_robot() { + let (model, chain, q_home) = load(); + 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); + let mut checker = attached_checker(&model, &chain); + + assert_eq!(checker.robot_contact_threshold(), 0.0); + assert_eq!(checker.payload_clearance_m(), PAYLOAD_CLEARANCE_M); + assert_eq!(checker.proxy_geom_name(), "cube"); + assert_eq!( + checker.environment_geom_names().collect::>(), + PAYLOAD_ENVIRONMENT + ); + for (label, q) in [("pick", &q_pick), ("place", &q_place)] { + assert!(!checker.robot_collides(q), "{label} robot collision"); + let distances = checker.payload_distances(q); + assert_eq!(distances.len(), PAYLOAD_ENVIRONMENT.len()); + assert!( + distances + .iter() + .all(|distance| !distance.violates_clearance()), + "{label} payload violation: {distances:?}" + ); + } + + let queried: HashSet<_> = checker + .environment_geom_names() + .map(|name| model.name_to_id(MjtObj::mjOBJ_GEOM, name).unwrap()) + .collect(); + let chain_bodies: HashSet<_> = chain.links().iter().map(|link| link.body_id).collect(); + for (geom_id, &body_id) in model.geom_bodyid().iter().enumerate() { + if body_id != 0 && chain_bodies.contains(&(body_id as usize)) { + assert!( + !queried.contains(&geom_id), + "robot geom {geom_id} unexpectedly entered payload pair scope" + ); + } + } + for excluded in ["cube", "place_pad"] { + let geom_id = model.name_to_id(MjtObj::mjOBJ_GEOM, excluded).unwrap(); + assert!( + !queried.contains(&geom_id), + "queried excluded geom {excluded}" + ); + } +} + +#[test] +fn attached_cube_blocks_straight_carry_and_planned_path_is_sampled_clear() { + let (model, chain, q_home) = load(); + 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); + let cfg = PlanConfig { + seed: SEED, + ..PlanConfig::default() + }; + assert_eq!(cfg.resolution, 0.05); + + let mut scratch = vec![0.0; chain.dof()]; + let mut robot_checker = attached_checker(&model, &chain); + let robot_blocks_straight = !edge_free( + &q_pick, + &q_place, + cfg.resolution, + &mut |q| robot_checker.robot_collides(q), + &mut scratch, + ); + let mut payload_checker = attached_checker(&model, &chain); + let payload_blocks_straight = !edge_free( + &q_pick, + &q_place, + cfg.resolution, + &mut |q| payload_checker.payload_collides(q), + &mut scratch, + ); + let mut combined_checker = attached_checker(&model, &chain); + assert!( + !edge_free( + &q_pick, + &q_place, + cfg.resolution, + &mut |q| combined_checker.collides(q), + &mut scratch, + ), + "fixture broken: combined straight carry is free" + ); + eprintln!( + "straight carry blockers: robot={robot_blocks_straight}, payload={payload_blocks_straight}" + ); + + let straight_samples = densify(&[q_pick.clone(), q_place.clone()], cfg.resolution); + let mut straight_audit = attached_checker(&model, &chain); + let mut straight_robot_collision_samples = 0usize; + let mut straight_payload_violation_samples = 0usize; + let mut straight_min_payload_distance_m = f64::INFINITY; + let mut straight_min_payload_pair = String::new(); + for q in &straight_samples { + straight_robot_collision_samples += usize::from(straight_audit.robot_collides(q)); + let distances = straight_audit.payload_distances(q); + straight_payload_violation_samples += usize::from( + distances + .iter() + .any(|distance| distance.violates_clearance()), + ); + for distance in distances { + if distance.distance_m < straight_min_payload_distance_m { + straight_min_payload_distance_m = distance.distance_m; + straight_min_payload_pair = distance.identity(); + } + } + } + + let mut planner_a = attached_checker(&model, &chain); + let plan_a = rrt_connect(&chain, &q_pick, &q_place, |q| planner_a.collides(q), &cfg); + assert_eq!(plan_a.status, PlanStatus::Success); + assert!(plan_a.waypoints.len() >= 3, "expected a carry dodge"); + + let mut audit = attached_checker(&model, &chain); + let mut planned_min_payload_distance_m = f64::INFINITY; + let mut planned_min_payload_pair = String::new(); + for (sample, q) in plan_a.path.iter().enumerate() { + assert!( + !audit.robot_collides(q), + "robot collision at path sample {sample}" + ); + let distances = audit.payload_distances(q); + assert!( + distances + .iter() + .all(|distance| !distance.violates_clearance()), + "payload violation at path sample {sample}: {distances:?}" + ); + for distance in distances { + if distance.distance_m < planned_min_payload_distance_m { + planned_min_payload_distance_m = distance.distance_m; + planned_min_payload_pair = distance.identity(); + } + } + } + + eprintln!( + "attached carry audit: straight_samples={}, straight_robot_collision_samples={}, \ + straight_payload_violation_samples={}, straight_min_payload_distance_m={:.9}, \ + straight_min_payload_pair={}, \ + planned_waypoints={}, planned_samples={}, planned_min_payload_distance_m={:.9}, \ + planned_min_payload_pair={}, planned_cost_rad={:.9}", + straight_samples.len(), + straight_robot_collision_samples, + straight_payload_violation_samples, + straight_min_payload_distance_m, + straight_min_payload_pair, + plan_a.waypoints.len(), + plan_a.path.len(), + planned_min_payload_distance_m, + planned_min_payload_pair, + plan_a.cost, + ); + + let mut planner_b = attached_checker(&model, &chain); + let plan_b = rrt_connect(&chain, &q_pick, &q_place, |q| planner_b.collides(q), &cfg); + assert_eq!(plan_b.status, PlanStatus::Success); + assert_eq!(plan_a.waypoints, plan_b.waypoints); + assert_eq!(plan_a.path, plan_b.path); +} + +#[test] +fn attached_box_spec_rejects_invalid_pair_scopes() { + let (model, chain, _) = load(); + let pose = Isometry3::translation(0.0, 0.0, 0.035); + let make = |proxy: &str, environment: &[&str], clearance: f64| { + AttachedBoxCollisionChecker::new( + &model, + &chain, + AttachedBoxSpec::new(proxy, pose, environment.iter().copied(), clearance), + ) + }; + + assert!(matches!( + make("cube", &[], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::EmptyEnvironmentSet) + )); + assert!(matches!( + make("missing", &PAYLOAD_ENVIRONMENT, PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::UnknownProxyGeom(_)) + )); + assert!(matches!( + make("floor", &["table"], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::ProxyIsNotBox { .. }) + )); + assert!(matches!( + make("table", &["pillar"], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::ProxyBodyIsNotMocap { .. }) + )); + assert!(matches!( + make("cube", &["missing"], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::UnknownEnvironmentGeom(_)) + )); + assert!(matches!( + make("cube", &["floor", "floor"], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::DuplicateEnvironmentGeom(_)) + )); + assert!(matches!( + make("cube", &["cube"], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::ProxyInEnvironmentSet(_)) + )); + assert!(matches!( + make("cube", &["place_pad"], PAYLOAD_CLEARANCE_M), + Err(AttachedBoxError::EnvironmentGeomIsNotContactEnabled(_)) + )); + assert!(matches!( + make("cube", &["floor"], -0.001), + Err(AttachedBoxError::InvalidClearance(_)) + )); + assert!(matches!( + make("cube", &["floor"], f64::NAN), + Err(AttachedBoxError::InvalidClearance(_)) + )); + + let mut non_finite_pose = pose; + non_finite_pose.translation.vector.x = f64::NAN; + assert!(matches!( + AttachedBoxCollisionChecker::new( + &model, + &chain, + AttachedBoxSpec::new( + "cube", + non_finite_pose, + PAYLOAD_ENVIRONMENT, + PAYLOAD_CLEARANCE_M, + ), + ), + Err(AttachedBoxError::NonFiniteProxyTransform) + )); +} + +#[test] +fn attached_box_spec_rejects_robot_geom_in_environment_scope() { + let xml = r#" + + + + + + + + + + + + + + + "#; + let model = MjModel::from_xml_string(xml).unwrap(); + let chain = Chain::from_mujoco(&model, "mini", "tip", "attachment_site").unwrap(); + let result = AttachedBoxCollisionChecker::new( + &model, + &chain, + AttachedBoxSpec::new( + "proxy", + Isometry3::identity(), + ["robot_collision"], + PAYLOAD_CLEARANCE_M, + ), + ); + assert!(matches!( + result, + Err(AttachedBoxError::EnvironmentGeomBelongsToRobot(_)) + )); +} + +#[test] +fn attached_proxy_world_pose_matches_fk_with_nonzero_geom_local_pose() { + let xml = r#" + + + + + + + + + + + + + + + + "#; + let model = MjModel::from_xml_string(xml).unwrap(); + let chain = Chain::from_mujoco(&model, "mini", "tip", "attachment_site").unwrap(); + let proxy_in_ee = Isometry3::from_parts( + Translation3::new(0.031, -0.019, 0.047), + UnitQuaternion::from_euler_angles(0.17, -0.23, 0.31), + ); + let mut checker = AttachedBoxCollisionChecker::new( + &model, + &chain, + AttachedBoxSpec::new("proxy", proxy_in_ee, ["obstacle"], PAYLOAD_CLEARANCE_M), + ) + .unwrap(); + let q = [0.37]; + let expected = fk(&chain, &q) * proxy_in_ee; + let actual = checker.proxy_world_pose(&q); + + let translation_error = (expected.translation.vector - actual.translation.vector).norm(); + let rotation_error = expected.rotation.angle_to(&actual.rotation); + assert!( + translation_error < 1e-10, + "proxy translation error {translation_error:.3e} m" + ); + assert!( + rotation_error < 1e-10, + "proxy rotation error {rotation_error:.3e} rad" + ); +} + +#[test] +fn intended_robot_proxy_contact_is_excluded_from_combined_predicate() { + let xml = r#" + + + + + + + + + + + + + + + + "#; + let model = MjModel::from_xml_string(xml).unwrap(); + let chain = Chain::from_mujoco(&model, "mini", "tip", "attachment_site").unwrap(); + let q = [0.0]; + + let mut unscoped_robot = CollisionChecker::new(&model, &chain); + unscoped_robot.contact_threshold = 0.0; + assert!( + unscoped_robot.collides(&q), + "fixture broken: contact-enabled proxy does not overlap robot" + ); + + let mut checker = AttachedBoxCollisionChecker::new( + &model, + &chain, + AttachedBoxSpec::new( + "proxy", + Isometry3::identity(), + ["obstacle"], + PAYLOAD_CLEARANCE_M, + ), + ) + .unwrap(); + checker.set_robot_contact_threshold(0.0); + assert!(!checker.robot_collides(&q)); + assert!(!checker.payload_collides(&q)); + assert!(!checker.collides(&q)); +} diff --git a/docs/attached_payload_protocol.md b/docs/attached_payload_protocol.md new file mode 100644 index 0000000..56ab09b --- /dev/null +++ b/docs/attached_payload_protocol.md @@ -0,0 +1,72 @@ +# Attached-payload collision proxy: pre-result protocol + +This protocol is fixed before implementing or running the attached-payload +experiment. Its purpose is to close one narrow Demo 3 planning limitation +without changing the arm planner or overstating what a sampled simulator check +can prove. + +## Scope and fixed semantics + +- Scene: `assets/ur5e/scene_pickplace.xml`. +- Query: the existing Demo 3 carry from the deterministic pick-approach IK + solution to the deterministic place-approach IK solution. +- Planner seed: `20260816`. +- Joint-space edge sampling: 0.05 rad L2, unchanged from `PlanConfig`. +- Robot rule: preserve the existing carry predicate exactly: a sampled state is + rejected only when MuJoCo emits a robot-involved contact with signed distance + `< 0.0 m`. +- Payload proxy: reuse the scene's visual-only `cube` box geom (half-extents + 0.025 m). At every queried joint state, set its world transform to + `T_world_EE * T_EE_cube`, where `T_EE_cube` is a 0.035 m translation along EE + +Z with no relative rotation, matching the scripted Demo 3 weld. +- Payload pair scope: call MuJoCo's explicit `mj_geomDistance` wrapper only for + `cube` versus the named collision geoms `floor`, `table`, and `pillar`. +- Payload rule: reject a sampled state when any scoped signed pair distance is + strictly `< 0.005 m`. This 5 mm value is a declared planning buffer, not a + tolerance estimate or a hardware guarantee. +- Intended load/tool proximity is excluded by construction: no cube-versus- + robot pair is queried. In particular, wrist/load proximity cannot make the + payload predicate fail. Visual-only `place_pad` is also outside the pair set. +- Combined carry rule: reject when either the unchanged robot rule or the + payload rule rejects the state. All non-carry Demo 3 segments retain their + existing robot-only predicates and thresholds. + +The explicit pair-distance query is materially different from setting a global +positive `contact_threshold`: the latter only filters contacts MuJoCo already +emitted and applies to every robot pair. This protocol requests geometric +distance only for the three declared payload/environment pairs. + +## Predeclared regression gates + +The implementation is acceptable only if all of these deterministic gates +hold: + +1. The carry start and goal are free under both the unchanged robot predicate + and the new payload predicate. +2. The 0.05 rad sampled straight carry edge is blocked by the combined + predicate. The test records whether the robot, payload, or both are + responsible instead of attributing the result after the fact. +3. RRT-Connect with the fixed seed succeeds under the combined predicate, and + every state in its returned densified path is re-audited as robot-free and + payload-clear using independent checker state. +4. The configured pair scope is exactly `floor`, `table`, and `pillar`; it + excludes the cube itself, all chain-body geoms (including the wrist), and the + visual place pad. +5. Two fresh checkers and planners produce bit-identical waypoints and densified + paths for the fixed input and seed. Wall-clock timing is explicitly excluded + from determinism claims. +6. Invalid specifications fail at construction: unknown or non-box proxy, + proxy not attached to a mocap body, unknown/duplicate/non-contact environment + geom, robot geom in the environment set, non-finite transform, and negative + or non-finite clearance. + +## Claim boundary + +Passing establishes only sampled, discrete clearance of one rigid box proxy +against three named environment geoms at the planner's 0.05 rad joint-space +resolution. It is not continuous swept-volume collision detection, does not +model grasp uncertainty, cube motion relative to the EE, compliance, fingers, +contact-rich grasping, calibration error, or unlisted geometry, and does not +certify hardware safety. Payload-versus-robot self-collision is intentionally +not checked because the wrist/load attachment region would require a separately +defined allowed-contact model. diff --git a/docs/attached_payload_results.md b/docs/attached_payload_results.md new file mode 100644 index 0000000..8aac37c --- /dev/null +++ b/docs/attached_payload_results.md @@ -0,0 +1,74 @@ +# Demo 3 attached-payload proxy results + +The [protocol](attached_payload_protocol.md) was committed before implementation +or result generation as +`5c36fd461749888c3a9e9168205ff0283a4ed8ac`. The implementation keeps the +original arm collision predicate and RRT-Connect algorithm, then adds a second, +pair-scoped predicate during the carry only: the 50 mm cube proxy must remain at +least 5 mm from the named `floor`, `table`, and `pillar` geoms at sampled states. + +## Fixed-query result + +All figures below are deterministic geometry/path fields from seed `20260816`; +wall-clock time is not part of the reproducibility claim. + +| Gate | Result | +|---|---:| +| Pick-approach endpoint | robot-free and payload-clear | +| Place-approach endpoint | robot-free and payload-clear | +| Straight-edge samples at 0.05 rad L2 | 23 | +| Straight samples with robot collision | 9 | +| Straight samples violating payload margin | 11 | +| Straight minimum cube/environment signed distance | **-0.065709578 m**, cube vs pillar | +| Combined fixed-seed RRT result | **success** | +| Shortcut waypoints / densified samples | **6 / 47** | +| Densified path robot-collision samples | **0** | +| Densified path payload-margin violations | **0** | +| Densified path minimum cube/environment distance | **0.011848632 m**, cube vs pillar | +| Margin above the declared 0.005 m payload threshold | **0.006848632 m** at sampled states | +| Joint-space path cost | 2.251250420 rad | + +Both predicates independently block the naive straight edge (`robot=true`, +`payload=true`). Two fresh checker/planner instances produce bit-identical +shortcut waypoints and densified paths. A fresh third checker re-audits every +returned state rather than trusting the planner result object. + +The compiled-pose regression also uses a separate MJCF fixture with a nonzero +EE attachment transform and a nonzero proxy geom-local translation and +rotation. MuJoCo's resulting `geom_xpos`/`geom_xmat` agree with +`FK(q) * T_EE_proxy` to less than `1e-10` m and rad, demonstrating that the +checker positions the compiled geom—not merely the mocap body origin—as +declared. + +Two repeated headless Demo 3 runs planned the attached-cube carry in 7.8 ms and +5.4 ms, respectively. Both produced the same 6 shortcut waypoints, 3.09 s +timed carry, and 0.0067 rad worst joint-tracking error. The timing is +illustrative and can vary with the machine and load; the path fields and +audited outcomes are the deterministic evidence. + +## Negative outcomes retained + +- The naive straight carry is unsafe under both scoped checks: 9/23 sampled + states have a robot collision and 11/23 violate the cube margin. +- Adding the payload gate changes the old 4-waypoint carry into a 6-waypoint, + 47-sample path and increases the representative full-demo worst tracking + error from the earlier 0.0037 rad report to 0.0067 rad. Neither change is + hidden. +- A global 40 mm robot threshold is not used. The positive 5 mm rule applies + only to the declared cube/environment pairs; the original zero-penetration + robot rule remains unchanged during carry. + +## Claim limits + +This is a sampled discrete check at 0.05 rad joint-space spacing, not continuous +swept-volume collision detection. The proxy is one rigid box at a fixed EE +transform. It does not model grasp uncertainty, object slip, compliance, +fingers, calibration error, contact-rich grasping, or unlisted scene geometry. +Cube-versus-robot checks are intentionally excluded because the attachment +region needs an explicit allowed-contact model; the configured pair scope and +constructor tests prevent wrist geoms from entering the environment list by +accident. A contact-enabled synthetic fixture additionally proves that an +actual penetrating robot/proxy contact is excluded while the independently +scoped payload/environment predicate remains active. The result is simulation +evidence for one deterministic query, not a workspace-wide or hardware-safety +certificate.