Stop trajectories at shortcut corners

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

@@ -2,7 +2,7 @@
 //!
 //! Joint-space RRT-Connect plans a collision-free path for the UR5e around a
 //! pillar that blocks the straight-line interpolant from home to a panned
-//! goal. The path is shortcut, densified, timed with a jerk-bounded scalar S-curve,
+//! goal. The shortcut edges are timed with rest-to-rest, jerk-bounded S-curves
 //! and tracked with a gravity-compensated PD servo (velocity feedforward
 //! through the Menagerie position actuators).
 //!
@@ -101,7 +101,7 @@ fn main() {
         a_max: 1.8,
         j_max: 8.0,
     };
-    let traj = time_parameterize(&plan.path, &limits, dt);
+    let traj = time_parameterize(&plan.waypoints, &limits, dt);
     println!(
         "[demo2] S-curve: {:.2} s · {} samples @ dt={dt} · limits v≤{} a≤{} j≤{}",
         traj.duration,

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

@@ -2,7 +2,7 @@
 //!
 //! IK solves grasp poses, RRT-Connect carries the cube around a pillar that
 //! blocks the joint-space interpolant, a pair-scoped attached-box proxy checks
-//! the carried cube against the environment, a jerk-bounded scalar S-curve
+//! the carried cube against the environment, jerk-bounded corner-stop S-curves
 //! times every segment, and a mocap weld stands in for a gripper (scripted
 //! attach, not contact-rich grasping).
 //!
@@ -334,7 +334,7 @@ fn go_with_collision(
         "{name}: planner failed ({:?})",
         plan.status
     );
-    let traj = time_parameterize(&plan.path, &ctx.limits, ctx.dt);
+    let traj = time_parameterize(&plan.waypoints, &ctx.limits, ctx.dt);
     println!(
         "[demo3] {name}: plan {plan_ms:.1} ms · {} waypoints · S-curve {:.2} s",
         plan.waypoints.len(),

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

@@ -359,7 +359,7 @@ fn main() {
                     seed
                 );
                 let trajectory =
-                    time_parameterize(&plan.path, &LIMITS, nominal_model.opt().timestep);
+                    time_parameterize(&plan.waypoints, &LIMITS, nominal_model.opt().timestep);
                 if seed == CANONICAL_SEED {
                     canonical_trajectory = Some(trajectory.clone());
                 }
@@ -1032,7 +1032,7 @@ fn render_artifacts(planning: &[PlanningRow], tracking: &[TrackingRow]) -> Artif
          - Only position PD and its desired-velocity-feedforward variant are compared here. This extension does not show that nominal-bias or integral-residual results generalize across queries.\n\
          - The combined plant is one deterministic condition: 1 kg payload at 0.10 m, 80% actuator gains, +1 Nms/rad joint damping, 10 ms command latency, and a 10 Nm / 120 ms shoulder-lift pulse at 45% of each trajectory. It is not a randomized uncertainty distribution.\n\
          - Planner collision checks remain discrete at 0.05 rad in joint-space L2 and use MuJoCo's emitted-contact set. The repository's positive contact threshold filters emitted candidates; with these zero-margin geoms it does not establish positive geometric clearance. Planner behavior is retained, but no 1-mm-clearance claim is made.\n\
-         - The execution gate checks signed distance `< 0` at each 2-ms simulation state in settle, path, and hold. It detects sampled penetration, not positive-distance near misses or continuous swept-volume collision between samples. Polyline corners remain unblended, so the scalar time law does not certify global acceleration or jerk.\n\
+         - 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. Each shortcut edge uses a rest-to-rest S-curve, making position, velocity, and acceleration continuous at full-stop corners while keeping jerk bounded almost everywhere; this is not geometric corner blending or a time-optimal parameterization.\n\
          - There is no sensor noise, contact-rich grasping, hardware experiment, or sim-to-real guarantee.\n\n\
          ## Reproduce\n\n\
          ```bash\n\

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

@@ -268,7 +268,7 @@ fn main() {
                     {
                         canonical_trajectory = Some((
                             seed,
-                            time_parameterize(&plan.path, &LIMITS, model.opt().timestep),
+                            time_parameterize(&plan.waypoints, &LIMITS, model.opt().timestep),
                         ));
                     }
                     planning.push(PlanningRow {
@@ -1028,6 +1028,7 @@ fn render_report(
          - Wilson intervals describe repeated draws from this declared conditional generator only. The generator, scenes, and robot model remain fixed.\n\
          - Direct and RRT collision checks sample joint-space edges at 0.05-rad L2 spacing and use the existing MuJoCo emitted-contact predicate. They do not certify continuous collision avoidance or positive clearance.\n\
          - Tracking uses one canonical successful path per accepted query, the nominal plant only, and sampled `dist < 0` execution checks at 2-ms states. Planning failures receive no tracking replay and remain visible in the denominator.\n\
+         - Trajectories use a rest-to-rest S-curve on every shortcut edge. Full-stop corners make joint position, velocity, and acceleration continuous and keep jerk bounded almost everywhere, but they are not geometric blends or time-optimal trajectories.\n\
          - The goal-bias comparison changes one planner field; it is an ablation, not a comparison with an independent planning implementation.\n\
          - There is no randomized plant uncertainty, sensing, localization, grasping, payload, hardware, or sim-to-real evidence in this extension.\n\n\
          ## Reproduce\n\n\

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

@@ -1,8 +1,8 @@
 //! Deterministic simulation-side robustness envelope for the UR5e tracker.
 //!
-//! The same collision-free path and jerk-bounded scalar time law are replayed against
-//! payload, actuator, damping, command-latency, and disturbance shifts.  The
-//! model-based controllers only receive bias forces from the unperturbed
+//! The same collision-free path and per-edge corner-stop S-curves are replayed
+//! against payload, actuator, damping, command-latency, and disturbance shifts.
+//! The model-based controllers only receive bias forces from the unperturbed
 //! nominal model, so perturbed runs do not leak the changed plant parameters
 //! into the controller.
 //!
@@ -255,7 +255,7 @@ fn benchmark_trajectory(model: &MjModel, chain: &Chain) -> Trajectory {
     );
     assert_eq!(plan.status, PlanStatus::Success, "benchmark plan failed");
     time_parameterize(
-        &plan.path,
+        &plan.waypoints,
         &TrajLimits {
             v_max: 0.55,
             a_max: 1.8,
@@ -569,7 +569,7 @@ fn write_artifacts(rows: &[Metrics], trajectory: &Trajectory) {
 
     let mut markdown = format!(
         "# UR5e controller robustness envelope (simulation)\n\n\
-         Deterministic MuJoCo stress test over a {:.2} s, {}-sample collision-free RRT-Connect path timed by the in-repo scalar S-curve. Polyline corners are not blended, so the complete joint trajectory is not globally acceleration- or jerk-bounded. **This is simulation evidence, not hardware validation or a sim-to-real guarantee.**\n\n\
+         Deterministic MuJoCo stress test over a {:.2} s, {}-sample collision-free RRT-Connect path timed by the in-repo per-edge S-curves. Every shortcut corner is a full stop: joint position, velocity, and acceleration are continuous, and jerk is bounded almost everywhere, although jerk may jump between finite values and the path is not geometrically blended or time-optimal. **This is simulation evidence, not hardware validation or a sim-to-real guarantee.**\n\n\
          Pass threshold (declared in code): RMS joint error <= {:.2} rad, maximum joint error <= {:.2} rad, and final joint error <= {:.2} rad. Controller bias forces always come from the unchanged nominal model; perturbed plant parameters are not exposed to the controller.\n\n\
          | Scenario | Controller | RMS joint (rad) | Max joint (rad) | Final (rad) | Max EE pos (m) | Peak force/limit | Saturated steps | Result |\n\
          |---|---|---:|---:|---:|---:|---:|---:|:---:|\n",

crates/arm-lab/src/lib.rs

@@ -4,9 +4,9 @@
 //!
 //! This crate is deliberately *not* a wrapper around a kinematics or planning
 //! library. The core algorithms — forward kinematics, geometric Jacobians,
-//! damped least-squares inverse kinematics, RRT-Connect, and jerk-limited
-//! time-parameterization — are implemented here directly. External crates are
-//! used only at the edges:
+//! damped least-squares inverse kinematics, RRT-Connect, and full-stop S-curve
+//! time-parameterization with almost-everywhere jerk bounds — are implemented
+//! here directly. External crates are used only at the edges:
 //!
 //! - [`mujoco_rs::MjModel`] supplies the robot description: the chain is
 //!   *extracted* from MuJoCo's compiled model (body offsets, joint axes,

crates/arm-lab/src/traj.rs

@@ -1,18 +1,16 @@
 //! Jerk-limited time-parameterization of joint-space paths, from scratch.
 //!
 //! A rest-to-rest **7-phase S-curve** (bang-bang jerk: trapezoidal
-//! acceleration) parameterizes the scalar path length `s ∈ [0, L]` of a
-//! polyline. Joint motion is the polyline's tangent times `(ṡ, s̈)`.
-//! Along each edge the tangent is constant, so joint jerk equals path jerk
-//! times the tangent; at the polyline's corners the tangent is discontinuous
-//! and a single sample can show an acceleration spike. The planner's
-//! shortcutting keeps the number of corners small (typically one or two).
+//! acceleration) parameterizes every nonzero edge of a joint-space polyline.
+//! Every interior waypoint is a full stop, so position, velocity, and
+//! acceleration are continuous even when the polyline tangent changes. Jerk
+//! is bounded almost everywhere; it can have finite one-sided jumps at phase
+//! and waypoint boundaries, where the classical derivative need not exist.
 //!
-//! Limits are specified per joint. Path-space `(v, a, j)` limits are the
-//! tightest joint limit divided by that joint's peak `|dqᵢ/ds|` on the path,
-//! so no joint exceeds its bound along an edge.
-
-use crate::plan::path_length;
+//! Limits are specified per joint. Each edge's path-space `(v, a, j)` limits
+//! are the tightest joint limit divided by `|dqᵢ/ds|` on that edge. Edge
+//! durations are rounded up to the fixed sample interval by slowing their
+//! S-curves, which can only reduce all three derivatives.
 
 /// Per-joint kinematic limits. One value is used for every joint; the path
 /// converter then tightens the scalar profile against the steepest joint.
@@ -42,6 +40,7 @@ pub struct Trajectory {
     pub q: Vec<Vec<f64>>,
     pub qd: Vec<Vec<f64>>,
     pub qdd: Vec<Vec<f64>>,
+    pub qddd: Vec<Vec<f64>>,
 }
 
 impl Trajectory {
@@ -54,58 +53,90 @@ impl Trajectory {
     }
 }
 
-/// Time-parameterize a joint-space polyline with a rest-to-rest S-curve.
+/// Time-parameterize a joint-space polyline with a rest-to-rest S-curve on
+/// every nonzero edge.
+///
+/// Interior waypoints are full stops. Callers should therefore pass geometric
+/// shortcut waypoints rather than collision-check densification samples.
 pub fn time_parameterize(path: &[Vec<f64>], limits: &TrajLimits, dt: f64) -> Trajectory {
     assert!(dt > 0.0, "dt must be positive");
     assert!(!path.is_empty(), "path must not be empty");
     let n = path[0].len();
     assert!(path.iter().all(|q| q.len() == n), "ragged path");
 
-    let length = path_length(path);
-    if length < 1e-12 {
+    if path.windows(2).all(|w| l2(&w[0], &w[1]) < 1e-12) {
         return stationary(path[0].clone(), dt);
     }
 
-    let slope = max_slope(path);
-    let mut v_s = f64::INFINITY;
-    let mut a_s = f64::INFINITY;
-    let mut j_s = f64::INFINITY;
-    for s in &slope {
-        if *s < 1e-12 {
+    let mut t = Vec::new();
+    let mut q = Vec::new();
+    let mut qd = Vec::new();
+    let mut qdd = Vec::new();
+    let mut qddd = Vec::new();
+    let mut elapsed_steps = 0usize;
+    let mut have_edge = false;
+
+    for edge in path.windows(2) {
+        let length = l2(&edge[0], &edge[1]);
+        if length < 1e-12 {
             continue;
         }
-        v_s = v_s.min(limits.v_max / s);
-        a_s = a_s.min(limits.a_max / s);
-        j_s = j_s.min(limits.j_max / s);
+        let tangent: Vec<f64> = edge[0]
+            .iter()
+            .zip(edge[1].iter())
+            .map(|(a, b)| (b - a) / length)
+            .collect();
+        let (v_s, a_s, j_s) = scalar_limits(&tangent, limits);
+        let curve = SCurve1d::rest_to_rest(length, v_s, a_s, j_s);
+        let edge_steps = ((curve.duration / dt).ceil() as usize).max(1);
+        let aligned_duration = edge_steps as f64 * dt;
+        let time_scale = aligned_duration / curve.duration;
+
+        let first_step = usize::from(have_edge);
+        for edge_step in first_step..=edge_steps {
+            let curve_t = (edge_step as f64 * dt / time_scale).min(curve.duration);
+            let motion = curve.at(curve_t);
+            let mut qk: Vec<f64> = edge[0]
+                .iter()
+                .zip(tangent.iter())
+                .map(|(start, direction)| start + direction * motion.s)
+                .collect();
+            if edge_step == edge_steps {
+                qk.clone_from(&edge[1]);
+            }
+            t.push((elapsed_steps + edge_step) as f64 * dt);
+            q.push(qk);
+            qd.push(
+                tangent
+                    .iter()
+                    .map(|direction| direction * motion.v / time_scale)
+                    .collect(),
+            );
+            qdd.push(
+                tangent
+                    .iter()
+                    .map(|direction| direction * motion.a / time_scale.powi(2))
+                    .collect(),
+            );
+            qddd.push(
+                tangent
+                    .iter()
+                    .map(|direction| direction * motion.j / time_scale.powi(3))
+                    .collect(),
+            );
+        }
+        elapsed_steps += edge_steps;
+        have_edge = true;
     }
-    assert!(
-        v_s.is_finite() && a_s.is_finite() && j_s.is_finite(),
-        "path has no moving joint"
-    );
 
-    let curve = SCurve1d::rest_to_rest(length, v_s, a_s, j_s);
-    let poly = Polyline::new(path);
-    let n_steps = (curve.duration / dt).ceil() as usize;
-    let mut t = Vec::with_capacity(n_steps + 1);
-    let mut q = Vec::with_capacity(n_steps + 1);
-    let mut qd = Vec::with_capacity(n_steps + 1);
-    let mut qdd = Vec::with_capacity(n_steps + 1);
-    for k in 0..=n_steps {
-        let tk = (k as f64 * dt).min(curve.duration);
-        let m = curve.at(tk);
-        let (qk, tang) = poly.at(m.s);
-        t.push(tk);
-        q.push(qk);
-        qd.push(tang.iter().map(|d| d * m.v).collect());
-        qdd.push(tang.iter().map(|d| d * m.a).collect());
-    }
     Trajectory {
         dt,
-        duration: curve.duration,
+        duration: elapsed_steps as f64 * dt,
         t,
         q,
         qd,
         qdd,
+        qddd,
     }
 }
 
@@ -118,22 +149,24 @@ fn stationary(q0: Vec<f64>, dt: f64) -> Trajectory {
         q: vec![q0],
         qd: vec![vec![0.0; n]],
         qdd: vec![vec![0.0; n]],
+        qddd: vec![vec![0.0; n]],
     }
 }
 
-fn max_slope(path: &[Vec<f64>]) -> Vec<f64> {
-    let n = path[0].len();
-    let mut m = vec![0.0; n];
-    for w in path.windows(2) {
-        let len = l2(&w[0], &w[1]);
-        if len < 1e-15 {
-            continue;
-        }
-        for (i, slot) in m.iter_mut().enumerate() {
-            *slot = f64::max(*slot, (w[1][i] - w[0][i]).abs() / len);
+fn scalar_limits(tangent: &[f64], limits: &TrajLimits) -> (f64, f64, f64) {
+    let mut scalar = (f64::INFINITY, f64::INFINITY, f64::INFINITY);
+    for slope in tangent.iter().map(|value| value.abs()) {
+        if slope >= 1e-12 {
+            scalar.0 = scalar.0.min(limits.v_max / slope);
+            scalar.1 = scalar.1.min(limits.a_max / slope);
+            scalar.2 = scalar.2.min(limits.j_max / slope);
         }
     }
-    m
+    assert!(
+        scalar.0.is_finite() && scalar.1.is_finite() && scalar.2.is_finite(),
+        "edge has no moving joint"
+    );
+    scalar
 }
 
 fn l2(a: &[f64], b: &[f64]) -> f64 {
@@ -144,52 +177,6 @@ fn l2(a: &[f64], b: &[f64]) -> f64 {
         .sqrt()
 }
 
-struct Polyline {
-    pts: Vec<Vec<f64>>,
-    cum: Vec<f64>,
-}
-
-impl Polyline {
-    fn new(path: &[Vec<f64>]) -> Self {
-        let mut cum = vec![0.0; path.len()];
-        for i in 1..path.len() {
-            cum[i] = cum[i - 1] + l2(&path[i - 1], &path[i]);
-        }
-        Self {
-            pts: path.to_vec(),
-            cum,
-        }
-    }
-
-    fn at(&self, s: f64) -> (Vec<f64>, Vec<f64>) {
-        let s_max = *self.cum.last().unwrap_or(&0.0);
-        let s = s.clamp(0.0, s_max);
-        let n = self.pts[0].len();
-        if s_max < 1e-15 {
-            return (self.pts[0].clone(), vec![0.0; n]);
-        }
-        let last = self.pts.len() - 1;
-        for i in 0..last {
-            if s <= self.cum[i + 1] + 1e-15 || i + 1 == last {
-                let seg = self.cum[i + 1] - self.cum[i];
-                if seg < 1e-15 {
-                    return (self.pts[i + 1].clone(), vec![0.0; n]);
-                }
-                let tau = ((s - self.cum[i]) / seg).clamp(0.0, 1.0);
-                let mut q = vec![0.0; n];
-                let mut tang = vec![0.0; n];
-                for k in 0..n {
-                    let d = self.pts[i + 1][k] - self.pts[i][k];
-                    q[k] = self.pts[i][k] + tau * d;
-                    tang[k] = d / seg;
-                }
-                return (q, tang);
-            }
-        }
-        (self.pts[last].clone(), vec![0.0; n])
-    }
-}
-
 /// 1D rest-to-rest jerk-limited profile.
 #[derive(Debug, Clone)]
 pub struct SCurve1d {
@@ -269,6 +256,14 @@ impl SCurve1d {
             };
         }
         let t = t.clamp(0.0, self.duration);
+        if t >= self.duration {
+            return Motion1d {
+                s: self.distance,
+                v: 0.0,
+                a: 0.0,
+                j: 0.0,
+            };
+        }
         let mut s = 0.0;
         let mut v = 0.0;
         let mut a = 0.0;