repose_trajectory/
quintic.rs

1//! Hermite reconstruction of stored joint knots and its exact limits.
2//!
3//! A [`CuTrajectoryPayload`] stores position, velocity and acceleration at
4//! every knot. Between two knots the motion is the unique quintic matching all
5//! six boundary values, which makes position, velocity *and* acceleration
6//! continuous across the whole trajectory, so joint jerk is finite everywhere.
7//! The cubic Hermite through positions and velocities alone is the lighter
8//! reconstruction used by the cubic follower mode.
9//!
10//! The peaks of velocity, acceleration and jerk of the quintic are computed in
11//! closed form ([`CuTrajectoryPayload::quintic_limit_ratio`]), and the velocity
12//! and acceleration peaks of the cubic in [`cubic_limit_ratio`]. Stretching all
13//! knot times by a factor `k` ([`CuTrajectoryPayload::stretch_time`]) keeps the shape and scales the peaks by
14//! `1/k`, `1/k^2` and `1/k^3`, so a trajectory whose ratio to the limits is
15//! `r > 1` satisfies them exactly after stretching by `r`.
16
17use crate::CuJointTrajectoryLimits;
18use crate::CuTrajectoryPayload;
19use cu29::prelude::*;
20use cu29::units::si::f64::Time;
21use cu29::units::si::time::second;
22use repose_kinematics::CuJointAcceleration;
23use repose_kinematics::CuJointPosition;
24use repose_kinematics::CuJointVelocity;
25
26/// Quintic through two knots over duration `t`, in normalized time `u = tau / t`.
27#[derive(Debug, Clone, Copy)]
28pub(crate) struct Quintic {
29    q0: f64,
30    v0: f64,
31    a0: f64,
32    t: f64,
33    c3: f64,
34    c4: f64,
35    c5: f64,
36}
37
38impl Quintic {
39    /// Quintic matching `(q, v, a)` at both ends of a segment of duration `t > 0`.
40    pub(crate) fn new(q: [f64; 2], v: [f64; 2], a: [f64; 2], t: f64) -> Self {
41        let r0 = q[1] - q[0] - v[0] * t - 0.5 * a[0] * t * t;
42        let r1 = (v[1] - v[0] - a[0] * t) * t;
43        let r2 = (a[1] - a[0]) * t * t;
44        Self {
45            q0: q[0],
46            v0: v[0],
47            a0: a[0],
48            t,
49            c3: 10.0 * r0 - 4.0 * r1 + 0.5 * r2,
50            c4: -15.0 * r0 + 7.0 * r1 - r2,
51            c5: 6.0 * r0 - 3.0 * r1 + 0.5 * r2,
52        }
53    }
54
55    /// Position, velocity and acceleration at `u` in `[0, 1]`.
56    pub(crate) fn eval(&self, u: f64) -> (f64, f64, f64) {
57        let t = self.t;
58        let position = self.q0
59            + u * (self.v0 * t
60                + u * (0.5 * self.a0 * t * t + u * (self.c3 + u * (self.c4 + u * self.c5))));
61        let velocity = self.v0
62            + self.a0 * t * u
63            + u * u * (3.0 * self.c3 + u * (4.0 * self.c4 + 5.0 * u * self.c5)) / t;
64        let acceleration =
65            self.a0 + u * (6.0 * self.c3 + u * (12.0 * self.c4 + 20.0 * u * self.c5)) / (t * t);
66        (position, velocity, acceleration)
67    }
68
69    fn jerk(&self, u: f64) -> f64 {
70        (6.0 * self.c3 + u * (24.0 * self.c4 + 60.0 * u * self.c5)) / (self.t * self.t * self.t)
71    }
72
73    /// Exact peaks of `(|velocity|, |acceleration|, |jerk|)` over the segment.
74    ///
75    /// Jerk is quadratic (peak at an end or its vertex), acceleration cubic
76    /// (peak where jerk vanishes), velocity quartic (peak where acceleration
77    /// vanishes; those roots are bracketed between the jerk roots and bisected).
78    pub(crate) fn peaks(&self) -> (f64, f64, f64) {
79        let mut jerk = libm::fabs(self.jerk(0.0)).max(libm::fabs(self.jerk(1.0)));
80        if libm::fabs(self.c5) > 1e-300 {
81            let u = -self.c4 / (5.0 * self.c5);
82            if u > 0.0 && u < 1.0 {
83                jerk = jerk.max(libm::fabs(self.jerk(u)));
84            }
85        }
86        let turning = unit_roots(10.0 * self.c5, 4.0 * self.c4, self.c3);
87        let mut acceleration = libm::fabs(self.a0).max(libm::fabs(self.eval(1.0).2));
88        let mut cuts = [0.0_f64; 4];
89        let mut count = 1;
90        for u in sorted(turning).into_iter().flatten() {
91            acceleration = acceleration.max(libm::fabs(self.eval(u).2));
92            cuts[count] = u;
93            count += 1;
94        }
95        cuts[count] = 1.0;
96        count += 1;
97        let mut velocity = libm::fabs(self.v0).max(libm::fabs(self.eval(1.0).1));
98        for w in cuts[..count].windows(2) {
99            let (mut lo, mut hi) = (w[0], w[1]);
100            let (flo, fhi) = (self.eval(lo).2, self.eval(hi).2);
101            if flo * fhi < 0.0 {
102                for _ in 0..64 {
103                    let mid = 0.5 * (lo + hi);
104                    if (self.eval(mid).2 < 0.0) == (flo < 0.0) {
105                        lo = mid;
106                    } else {
107                        hi = mid;
108                    }
109                }
110                velocity = velocity.max(libm::fabs(self.eval(0.5 * (lo + hi)).1));
111            }
112        }
113        (velocity, acceleration, jerk)
114    }
115}
116
117/// Roots of `a*u^2 + b*u + c` strictly inside `(0, 1)`.
118pub(crate) fn unit_roots(a: f64, b: f64, c: f64) -> [Option<f64>; 2] {
119    let inside = |u: f64| (u > 0.0 && u < 1.0).then_some(u);
120    if libm::fabs(a) <= 1e-300 {
121        if libm::fabs(b) <= 1e-300 {
122            return [None, None];
123        }
124        return [inside(-c / b), None];
125    }
126    let disc = b * b - 4.0 * a * c;
127    if disc < 0.0 {
128        return [None, None];
129    }
130    let root = libm::sqrt(disc);
131    [
132        inside((-b + root) / (2.0 * a)),
133        inside((-b - root) / (2.0 * a)),
134    ]
135}
136
137fn sorted(roots: [Option<f64>; 2]) -> [Option<f64>; 2] {
138    match roots {
139        [Some(a), Some(b)] if a > b => [Some(b), Some(a)],
140        other => other,
141    }
142}
143
144impl<const N: usize, const CAP: usize> CuTrajectoryPayload<N, CAP> {
145    /// Smallest uniform time stretch under which the quintic reconstruction of
146    /// this trajectory respects `limits`, as a ratio of peak to limit (dimensionless).
147    ///
148    /// Returns the largest of `peak_v / v_max`, `sqrt(peak_a / a_max)` and
149    /// `cbrt(peak_j / j_max)` over all segments and non-fixed joints (an infinite
150    /// jerk limit contributes nothing). A result `<= 1` means the limits already
151    /// hold exactly. Returns `None` for fewer than two knots, non-positive
152    /// segment durations, non-finite values, joint-kind disagreements or limits
153    /// that are not positive.
154    pub fn quintic_limit_ratio(&self, limits: &CuJointTrajectoryLimits<N>) -> Option<f64> {
155        let trajectory = self;
156        if trajectory.len() < 2 || trajectory.len() > CAP {
157            return None;
158        }
159        let mut ratio = 0.0_f64;
160        for pair in trajectory.points().windows(2) {
161            let (a, b) = (&pair[0], &pair[1]);
162            let t = b.time_from_start.get::<second>() - a.time_from_start.get::<second>();
163            if !t.is_finite() || t <= 0.0 {
164                return None;
165            }
166            for j in 0..N {
167                if matches!(a.positions[j], CuJointPosition::Fixed) {
168                    continue;
169                }
170                let kinds_agree = matches!(
171                    (
172                        a.positions[j],
173                        b.positions[j],
174                        a.velocities[j],
175                        b.velocities[j],
176                        limits.max_velocities[j]
177                    ),
178                    (
179                        CuJointPosition::Revolute(_),
180                        CuJointPosition::Revolute(_),
181                        CuJointVelocity::Revolute(_),
182                        CuJointVelocity::Revolute(_),
183                        CuJointVelocity::Revolute(_)
184                    ) | (
185                        CuJointPosition::Prismatic(_),
186                        CuJointPosition::Prismatic(_),
187                        CuJointVelocity::Prismatic(_),
188                        CuJointVelocity::Prismatic(_),
189                        CuJointVelocity::Prismatic(_)
190                    )
191                );
192                let (vmax, amax, jmax) = (
193                    limits.max_velocities[j].si_value(),
194                    limits.acc(j),
195                    limits.jerk(j),
196                );
197                if !kinds_agree
198                    || vmax.is_nan()
199                    || vmax <= 0.0
200                    || amax.is_nan()
201                    || amax <= 0.0
202                    || jmax.is_nan()
203                    || jmax <= 0.0
204                {
205                    return None;
206                }
207                let quintic = Quintic::new(
208                    [a.positions[j].si_value(), b.positions[j].si_value()],
209                    [a.velocities[j].si_value(), b.velocities[j].si_value()],
210                    [a.accelerations[j].si_value(), b.accelerations[j].si_value()],
211                    t,
212                );
213                let (v, acc, jerk) = quintic.peaks();
214                if !v.is_finite() || !acc.is_finite() || !jerk.is_finite() {
215                    return None;
216                }
217                ratio = ratio
218                    .max(v / vmax)
219                    .max(libm::sqrt(acc / amax))
220                    .max(libm::cbrt(jerk / jmax));
221            }
222        }
223        Some(ratio)
224    }
225
226    /// Stretches all knot times by `factor` (dimensionless) and rescales
227    /// velocities by `1/factor` and accelerations by `1/factor^2`, so the motion
228    /// keeps its exact shape and every velocity, acceleration and jerk peak drops
229    /// by `1/factor`, `1/factor^2` and `1/factor^3`. `factor` must be finite and
230    /// positive.
231    pub fn stretch_time(&mut self, factor: f64) -> CuResult<()> {
232        if !factor.is_finite() || factor <= 0.0 {
233            return Err(CuError::from("Time stretch factor must be positive"));
234        }
235        let len = self.len();
236        for point in &mut self.points[..len] {
237            point.time_from_start =
238                Time::new::<second>(point.time_from_start.get::<second>() * factor);
239            for j in 0..N {
240                point.velocities[j] = CuJointVelocity::from_coordinate(
241                    point.positions[j],
242                    point.velocities[j].si_value() / factor,
243                );
244                point.accelerations[j] = CuJointAcceleration::from_coordinate(
245                    point.positions[j],
246                    point.accelerations[j].si_value() / (factor * factor),
247                );
248            }
249        }
250        Ok(())
251    }
252}
253
254/// Exact `(max speed, min speed, max |acceleration|)` of a cubic or quintic
255/// Hermite move over duration `t` with endpoint speeds `v0`, `v1`.
256///
257/// Cubic speed is quadratic (vertex) and acceleration linear (ends). Quintic
258/// (zero end accelerations) acceleration is `u·(6a3 + 12a4·u + 20a5·u^2)/T^2`
259/// and jerk quadratic, so all critical points are quadratic roots.
260pub(crate) fn polynomial_extrema(
261    quintic: bool,
262    length: f64,
263    v0: f64,
264    v1: f64,
265    t: f64,
266) -> (f64, f64, f64) {
267    let mut high = v0.max(v1);
268    let mut low = v0.min(v1);
269    let mut acc = 0.0_f64;
270    if quintic {
271        let start = length - v0 * t;
272        let end = (v1 - v0) * t;
273        let a3 = 10.0 * start - 4.0 * end;
274        let a4 = -15.0 * start + 7.0 * end;
275        let a5 = 6.0 * start - 3.0 * end;
276        let speed =
277            |u: f64| v0 + (3.0 * a3 * u * u + 4.0 * a4 * u * u * u + 5.0 * a5 * u * u * u * u) / t;
278        let accel = |u: f64| (6.0 * a3 * u + 12.0 * a4 * u * u + 20.0 * a5 * u * u * u) / (t * t);
279        for u in unit_roots(20.0 * a5, 12.0 * a4, 6.0 * a3)
280            .into_iter()
281            .flatten()
282        {
283            high = high.max(speed(u));
284            low = low.min(speed(u));
285        }
286        for u in unit_roots(60.0 * a5, 24.0 * a4, 6.0 * a3)
287            .into_iter()
288            .flatten()
289        {
290            acc = acc.max(libm::fabs(accel(u)));
291        }
292    } else {
293        let a = v0 * t;
294        let b = 3.0 * length - 2.0 * a - v1 * t;
295        let c = -2.0 * length + a + v1 * t;
296        if let Some(u) = unit_roots(0.0, 6.0 * c, 2.0 * b)[0] {
297            let speed = (a + u * (2.0 * b + 3.0 * c * u)) / t;
298            high = high.max(speed);
299            low = low.min(speed);
300        }
301        acc = libm::fabs(2.0 * b).max(libm::fabs(2.0 * b + 6.0 * c)) / (t * t);
302    }
303    (high, low, acc)
304}
305
306/// Smallest uniform time stretch under which the cubic Hermite reconstruction
307/// (positions and velocities) of `trajectory` respects the velocity and
308/// acceleration limits, as a ratio of peak to limit.
309///
310/// Returns the largest of `peak_v / v_max` and `sqrt(peak_a / a_max)` over all
311/// segments and non-fixed joints. A result `<= 1` means the limits already
312/// hold exactly. Returns `None` for fewer than two knots, non-positive
313/// segment durations, non-finite values, joint-kind disagreements or limits
314/// that are not positive.
315pub(crate) fn cubic_limit_ratio<const N: usize, const CAP: usize>(
316    trajectory: &CuTrajectoryPayload<N, CAP>,
317    limits: &CuJointTrajectoryLimits<N>,
318) -> Option<f64> {
319    if trajectory.len() < 2 || trajectory.len() > CAP {
320        return None;
321    }
322    let mut ratio = 0.0_f64;
323    for pair in trajectory.points().windows(2) {
324        let (a, b) = (&pair[0], &pair[1]);
325        let t = b.time_from_start.get::<second>() - a.time_from_start.get::<second>();
326        if !t.is_finite() || t <= 0.0 {
327            return None;
328        }
329        for j in 0..N {
330            if matches!(a.positions[j], CuJointPosition::Fixed) {
331                continue;
332            }
333            let kinds_agree = matches!(
334                (
335                    a.positions[j],
336                    b.positions[j],
337                    a.velocities[j],
338                    b.velocities[j],
339                    limits.max_velocities[j]
340                ),
341                (
342                    CuJointPosition::Revolute(_),
343                    CuJointPosition::Revolute(_),
344                    CuJointVelocity::Revolute(_),
345                    CuJointVelocity::Revolute(_),
346                    CuJointVelocity::Revolute(_)
347                ) | (
348                    CuJointPosition::Prismatic(_),
349                    CuJointPosition::Prismatic(_),
350                    CuJointVelocity::Prismatic(_),
351                    CuJointVelocity::Prismatic(_),
352                    CuJointVelocity::Prismatic(_)
353                )
354            );
355            let (vmax, amax) = (limits.max_velocities[j].si_value(), limits.acc(j));
356            if !kinds_agree || vmax.is_nan() || vmax <= 0.0 || amax.is_nan() || amax <= 0.0 {
357                return None;
358            }
359            let (high, low, acc) = polynomial_extrema(
360                false,
361                b.positions[j].si_value() - a.positions[j].si_value(),
362                a.velocities[j].si_value(),
363                b.velocities[j].si_value(),
364                t,
365            );
366            if !high.is_finite() || !low.is_finite() || !acc.is_finite() {
367                return None;
368            }
369            ratio = ratio
370                .max(high / vmax)
371                .max(-low / vmax)
372                .max(libm::sqrt(acc / amax));
373        }
374    }
375    Some(ratio)
376}
repose_trajectory/quintic.rs

repose_trajectory/
quintic.rs
