deke_linear/
retimer.rs

1//! Stage C — time-parameterise a joint path at constant TCP speed.
2//!
3//! This is a CNC-style constant-feedrate planner, not a TOPP retimer. The
4//! feasible-speed ceiling along the path (the "maximum velocity curve") comes from
5//! the per-joint v/a/j limits projected onto the path tangent `q'(s)`. The
6//! commanded speed `tcp_speed` is held flat wherever that ceiling allows; near a
7//! singularity `|q'(s)| → ∞` so the ceiling collapses and the feedrate dips to zero
8//! smoothly instead of demanding infinite joint speed. The profile is built by a
9//! backward+forward acceleration-bounded pass (zero speed at both ends) followed by
10//! a forward jerk-limited time integration that tracks it.
11//!
12//! Joint velocity is enforced exactly; acceleration and jerk are enforced through
13//! the tangent projection — the `q''(s)·ṡ²` curvature cross-term is a deliberate
14//! first-pass approximation, softened by the jerk-limited integrator.
15
16use std::time::Duration;
17
18use deke_types::glam::DVec3;
19use deke_types::{ContinuousFKChain, DekeError, SRobotPath, SRobotQ, SRobotTraj};
20
21use crate::constraints::LinearConstraints;
22use crate::diagnostic::LinearRetimerDiagnostic;
23use crate::error::LinearError;
24use crate::util::interp;
25
26const BIG: f64 = 1e9;
27
28/// Constant-feedrate, jerk-limited retimer over a joint path.
29#[derive(Clone, Debug)]
30pub struct ConstantSpeedRetimer<'a, const N: usize, FK> {
31    fk: &'a FK,
32}
33
34impl<'a, const N: usize, FK> ConstantSpeedRetimer<'a, N, FK>
35where
36    FK: ContinuousFKChain<N, f64>,
37{
38    pub fn new(fk: &'a FK) -> Self {
39        Self { fk }
40    }
41
42    pub fn retime_path(
43        &self,
44        c: &LinearConstraints<N>,
45        path: &SRobotPath<N, f64>,
46        run_idx: usize,
47    ) -> Result<(SRobotTraj<N, f64>, LinearRetimerDiagnostic), LinearError> {
48        let q: Vec<SRobotQ<N, f64>> = path.iter().copied().collect();
49        let m = q.len();
50        let dt = c.output_dt.as_secs_f64().max(1e-6);
51
52        // Cartesian arc length from FK end positions (true metres for `tcp_speed`).
53        let pos: Vec<DVec3> = q
54            .iter()
55            .map(|qi| self.fk.fk_end(qi).map(|t| t.translation))
56            .collect::<Result<_, DekeError>>()?;
57        let mut s = vec![0.0f64; m];
58        for i in 1..m {
59            s[i] = s[i - 1] + pos[i].distance(pos[i - 1]);
60        }
61        let total = s[m - 1];
62        if m < 2 || total < 1e-9 {
63            let traj = SRobotTraj::new(c.output_dt, path.clone());
64            return Ok((
65                traj,
66                LinearRetimerDiagnostic {
67                    output_samples: m,
68                    duration: Duration::from_secs_f64((m.saturating_sub(1)) as f64 * dt),
69                    arc_length: total,
70                    commanded_speed: c.tcp_speed,
71                    peak_speed: 0.0,
72                },
73            ));
74        }
75
76        // q'(s) by central difference over arc length.
77        let mut qp = vec![SRobotQ::<N, f64>::zeros(); m];
78        for (i, qpi) in qp.iter_mut().enumerate() {
79            let (lo, hi) = if i == 0 {
80                (0, 1)
81            } else if i == m - 1 {
82                (m - 2, m - 1)
83            } else {
84                (i - 1, i + 1)
85            };
86            let ds = (s[hi] - s[lo]).max(1e-12);
87            *qpi = (q[hi] - q[lo]) * (1.0 / ds);
88        }
89
90        // Raw geometric speed ceiling per sample (before clamping to the command).
91        // A dip below the command here is forced by joint v-limits + path curvature,
92        // i.e. a corner or near-singular patch — distinct from the temporal rest
93        // ramps the MVC adds at the ends.
94        if c.forbid_interior_dips {
95            let mut worst: Option<(usize, f64)> = None;
96            #[allow(clippy::needless_range_loop)]
97            for i in 1..m - 1 {
98                let g = project_min(&qp[i], &c.joint.v_max);
99                if g < c.tcp_speed * (1.0 - 1e-3) && worst.is_none_or(|(_, gw)| g < gw) {
100                    worst = Some((i, g));
101                }
102            }
103            if let Some((i, g)) = worst {
104                return Err(LinearError::SpeedDipRequired {
105                    run: run_idx,
106                    s: s[i],
107                    feasible_speed: g,
108                    commanded: c.tcp_speed,
109                });
110            }
111        }
112
113        let v_ceiling = |i: usize| project_min(&qp[i], &c.joint.v_max).min(c.tcp_speed);
114        let a_path: Vec<f64> = (0..m)
115            .map(|i| project_min(&qp[i], &c.joint.a_max))
116            .collect();
117        let j_path: Vec<f64> = (0..m)
118            .map(|i| project_min(&qp[i], &c.joint.j_max))
119            .collect();
120
121        // Acceleration-bounded velocity ceiling. Only the end is pinned to rest;
122        // start-from-rest is the integrator's initial condition (v = 0), not a
123        // ceiling — pinning the start too would forbid ever accelerating.
124        let mut vc: Vec<f64> = (0..m).map(v_ceiling).collect();
125        vc[m - 1] = 0.0;
126        for i in (0..m - 1).rev() {
127            let ds = s[i + 1] - s[i];
128            vc[i] = vc[i].min((vc[i + 1] * vc[i + 1] + 2.0 * a_path[i] * ds).sqrt());
129        }
130        for i in 1..m {
131            let ds = s[i] - s[i - 1];
132            vc[i] = vc[i].min((vc[i - 1] * vc[i - 1] + 2.0 * a_path[i - 1] * ds).sqrt());
133        }
134
135        // Forward jerk-limited time integration tracking the ceiling.
136        let mut samples: Vec<SRobotQ<N, f64>> = vec![q[0]];
137        let mut sx = 0.0f64;
138        let mut v = 0.0f64;
139        let mut a = 0.0f64;
140        let mut peak = 0.0f64;
141        let max_iters = ((total / (c.tcp_speed.max(1e-6) * dt)) as usize + m) * 8 + 100_000;
142
143        let mut iters = 0usize;
144        while sx < total - 1e-9 {
145            iters += 1;
146            if iters > max_iters {
147                return Err(LinearError::Stalled {
148                    run: run_idx,
149                    s: sx,
150                });
151            }
152            let vlim = interp(&s, &vc, sx).max(0.0);
153            let alim = interp(&s, &a_path, sx);
154            let jlim = interp(&s, &j_path, sx);
155
156            let a_des = ((vlim - v) / dt).clamp(-alim, alim);
157            a = a_des.clamp(a - jlim * dt, a + jlim * dt).clamp(-alim, alim);
158            v = (v + a * dt).clamp(0.0, vlim);
159            peak = peak.max(v);
160            sx += v * dt;
161            samples.push(sample_at(&q, &s, sx.min(total)));
162
163            // Guard against a stall at a vanishing ceiling (true singularity).
164            if v < 1e-9 && vlim < 1e-9 && sx < total - 1e-6 {
165                return Err(LinearError::Stalled {
166                    run: run_idx,
167                    s: sx,
168                });
169            }
170        }
171        if samples.last().map(|l| l.distance(&q[m - 1])).unwrap_or(1.0) > 1e-9 {
172            samples.push(q[m - 1]);
173        }
174
175        let out_samples = samples.len();
176        let path_out = SRobotPath::try_new(samples).map_err(LinearError::from)?;
177        let traj = SRobotTraj::new(c.output_dt, path_out);
178        Ok((
179            traj,
180            LinearRetimerDiagnostic {
181                output_samples: out_samples,
182                duration: Duration::from_secs_f64((out_samples.saturating_sub(1)) as f64 * dt),
183                arc_length: total,
184                commanded_speed: c.tcp_speed,
185                peak_speed: peak,
186            },
187        ))
188    }
189}
190
191/// `min_j limit_j / |qp_j|` over axes that actually move; `BIG` if none do.
192fn project_min<const N: usize>(qp: &SRobotQ<N, f64>, limit: &SRobotQ<N, f64>) -> f64 {
193    let mut m = BIG;
194    let mut any = false;
195    for j in 0..N {
196        let g = qp.0[j].abs();
197        if g > 1e-9 {
198            any = true;
199            m = m.min(limit.0[j] / g);
200        }
201    }
202    if any { m } else { BIG }
203}
204
205fn sample_at<const N: usize>(q: &[SRobotQ<N, f64>], s: &[f64], x: f64) -> SRobotQ<N, f64> {
206    let x = x.clamp(s[0], s[s.len() - 1]);
207    match s.binary_search_by(|v| v.partial_cmp(&x).unwrap()) {
208        Ok(i) => q[i],
209        Err(i) => {
210            if i == 0 {
211                return q[0];
212            }
213            if i >= s.len() {
214                return q[s.len() - 1];
215            }
216            let (s0, s1) = (s[i - 1], s[i]);
217            let f = if s1 > s0 { (x - s0) / (s1 - s0) } else { 0.0 };
218            q[i - 1].interpolate(&q[i], f)
219        }
220    }
221}
deke_linear/retimer.rs

deke_linear/
retimer.rs
