copp\path/
mod.rs

1//! Path abstraction for trajectory planning.
2//!
3//! The main entry point is [`Path`]. Pick the constructor by the path data you own:
4//!
5//! | You have | Use | Notes |
6//! |---|---|---|
7//! | Analytic formula `q(s)` | [`Path::from_parametric`](crate::path::Path::from_parametric) | Computes derivatives up to third order by [`Jet3`] automatic differentiation. |
8//! | Waypoint positions | [`Path::from_waypoints`](crate::path::Path::from_waypoints) | Builds a spline from a `(dim x n_points)` waypoint matrix. |
9//! | Explicit `q/dq/ddq` evaluator | [`Path::from_evaluator_2nd`](crate::path::Path::from_evaluator_2nd) | Use for TOPP2/COPP2 or other workflows that do not need jerk. See [`PathEvaluator2nd`]. |
10//! | Explicit `q/dq/ddq/dddq` evaluator | [`Path::from_evaluator_3rd`](crate::path::Path::from_evaluator_3rd) | Use for TOPP3/COPP3 workflows. See [`PathEvaluator3rd`]. |
11//!
12//! Once built, call one of the evaluation methods with a one-dimensional parameter slice:
13//!
14//! | Method | Output |
15//! |---|---|
16//! | [`Path::evaluate_q`](crate::path::Path::evaluate_q)         | position `q` only |
17//! | [`Path::evaluate_up_to_2nd`](crate::path::Path::evaluate_up_to_2nd) | `q`, `dq`, `ddq` |
18//! | [`Path::evaluate_up_to_3rd`](crate::path::Path::evaluate_up_to_3rd) | `q`, `dq`, `ddq`, `dddq` |
19
20pub mod autodiff;
21mod path_core;
22pub mod spline;
23
24pub use autodiff::{Jet3, cos, exp, ln, powi, sin, sqrt};
25pub use path_core::{
26    ParametricFn, Path, PathDerivatives, PathEvaluator, PathEvaluator2nd, PathEvaluator3rd,
27};
28pub use spline::{Parametrization, SplineConfig};
29
30#[cfg(test)]
31use crate::diag::PathError;
32
33/// Policy for handling path queries outside the configured `s` range.
34#[derive(Clone, Copy, Debug)]
35pub enum OutOfRangeMode {
36    /// Return an error when s is outside `[s_min, s_max]`.
37    Error,
38    /// Silently clamp s to `[s_min, s_max]`.
39    Clamp,
40}
41
42/// Build a random Lissajous analytic path.
43///
44/// This helper is intended for cross-module unit tests to avoid repeating the
45/// same hand-written path construction logic.
46#[cfg(test)]
47#[allow(clippy::type_complexity)]
48pub(crate) fn lissajous_path_for_test(
49    dim: usize,
50    s_len: usize,
51    rng: &mut impl rand::RngExt,
52) -> Result<(nalgebra::DMatrix<f64>, Path, Vec<f64>, Vec<f64>), PathError> {
53    let omega = (0..dim)
54        .map(|_| rng.random_range(0.1..(2.0 * std::f64::consts::PI)))
55        .collect::<Vec<f64>>();
56    let phi = (0..dim)
57        .map(|_| rng.random_range(0.0..(2.0 * std::f64::consts::PI)))
58        .collect::<Vec<f64>>();
59
60    let (s, path) = lissajous_path_fixed_for_test(dim, s_len, omega.clone(), phi.clone())?;
61
62    Ok((s, path, omega, phi))
63}
64
65/// Build a Lissajous analytic path from fixed frequencies/phases.
66///
67/// This helper is intended for cross-module unit tests to avoid repeating the
68/// same hand-written path construction logic.
69#[cfg(test)]
70pub(crate) fn lissajous_path_fixed_for_test(
71    dim: usize,
72    s_len: usize,
73    omega: Vec<f64>,
74    phi: Vec<f64>,
75) -> Result<(nalgebra::DMatrix<f64>, Path), PathError> {
76    let s = nalgebra::DMatrix::<f64>::from_fn(1, s_len, |_, j| j as f64 / (s_len - 1) as f64);
77
78    let path = Path::from_parametric(
79        move |s: Jet3| {
80            (0..dim)
81                .map(|i| sin(omega[i] * s + phi[i]))
82                .collect::<Vec<Jet3>>()
83        },
84        0.0,
85        1.0,
86    )?;
87
88    Ok((s, path))
89}
90
91/// Add symmetric axial limits (`+limit` / `-limit`) to all currently stored stations.
92///
93/// The helper applies velocity, acceleration and jerk constraints in one call.
94#[cfg(test)]
95pub(crate) fn add_symmetric_axial_limits_for_test<M: crate::robot::robot_core::RobotBasic>(
96    robot: &mut crate::robot::robot_core::Robot<M>,
97    vel_limit: f64,
98    acc_limit: f64,
99    jerk_limit: Option<f64>,
100) -> Result<(), crate::diag::ConstraintError> {
101    let dim = robot.dim();
102    let n = robot.constraints.len();
103    if n == 0 {
104        return Ok(());
105    }
106
107    let vel_max = vec![vel_limit; dim];
108    let vel_min = vec![-vel_limit; dim];
109
110    let acc_max = vec![acc_limit; dim];
111    let acc_min = vec![-acc_limit; dim];
112
113    let robot = robot
114        .with_axial_velocity((vel_max.as_slice(), n), (vel_min.as_slice(), n), 0)?
115        .with_axial_acceleration((acc_max.as_slice(), n), (acc_min.as_slice(), n), 0)?;
116
117    if let Some(jerk_limit) = jerk_limit {
118        let jerk_max = vec![jerk_limit; dim];
119        let jerk_min = vec![-jerk_limit; dim];
120        robot.with_axial_jerk((jerk_max.as_slice(), n), (jerk_min.as_slice(), n), 0)?;
121    }
122
123    Ok(())
124}
125
126#[cfg(test)]
127mod tests {
128    use super::*;
129    use crate::copp::copp2::stable::basic::{Topp2ProblemBuilder, s_to_t_topp2};
130    use crate::copp::copp2::stable::reach_set2::ReachSet2OptionsBuilder;
131    use crate::copp::copp2::stable::topp2_ra::topp2_ra;
132    use crate::copp::copp3::stable::basic::{
133        Topp3ProblemBuilder, Topp3Profile, s_to_t_topp3, t_to_s_topp3,
134    };
135    use crate::copp::copp3::stable::topp3_lp::topp3_lp;
136    use crate::copp::{ClarabelOptionsBuilder, InterpolationMode};
137    use crate::diag::ConstraintError;
138    use crate::diag::Verbosity;
139    use crate::robot::robot_core::Robot;
140    use nalgebra::DMatrix;
141    use plotters::prelude::*;
142    use rand::RngExt;
143    use std::error::Error;
144    use std::fs::create_dir_all;
145    use std::path::Path;
146    use std::time::Instant;
147
148    const DIM: usize = 6;
149    const N: usize = 1000;
150
151    #[test]
152    fn test_topp3_with_spline_path() -> Result<(), Box<dyn Error>> {
153        crate::verbosity_log!(
154            crate::diag::Verbosity::Summary,
155            "\n=== TOPP3 with Spline Path (6-DOF) ==="
156        );
157
158        let n_waypoints = 10;
159        let start = Instant::now();
160        let waypoints = make_random_waypoints(DIM, n_waypoints);
161        let sample_ms = start.elapsed().as_secs_f64() * 1e3;
162
163        let start = Instant::now();
164        let path = super::Path::from_waypoints(&waypoints, SplineConfig::default())?;
165        let build_ms = start.elapsed().as_secs_f64() * 1e3;
166
167        let s = make_s_vector(N);
168        let start = Instant::now();
169        let derivs = path.evaluate_up_to_3rd(s.as_slice())?;
170        let eval_ms = start.elapsed().as_secs_f64() * 1e3;
171
172        crate::verbosity_log!(
173            crate::diag::Verbosity::Summary,
174            "[Waypoint Sample] {sample_ms:.3} ms, [Spline Build] {build_ms:.3} ms, [Spline Eval] {eval_ms:.3} ms"
175        );
176
177        let mut robot = setup_robot_with_constraints(&s, &derivs)?;
178        let s_slice: Vec<f64> = (0..N).map(|j| s[(0, j)]).collect();
179        let (profile, timings) = solve_topp_pipeline(&mut robot, &s_slice)?;
180
181        crate::verbosity_log!(
182            crate::diag::Verbosity::Summary,
183            "[TOPP2-RA] {:.3} ms, t_motion = {:.3} s",
184            timings.topp2_ms,
185            timings.t_motion_2
186        );
187        crate::verbosity_log!(
188            crate::diag::Verbosity::Summary,
189            "[TOPP3-LP] {:.3} ms, t_motion = {:.3} s",
190            timings.topp3_ms,
191            timings.t_motion_3
192        );
193
194        let (t, q_t, dq_t, ddq_t, dddq_t, interp_ms) =
195            interpolate_to_time_domain(&path, &s_slice, &profile, 1e-3)?;
196        crate::verbosity_log!(
197            crate::diag::Verbosity::Summary,
198            "[Interpolation] {:.3} ms, {} samples",
199            interp_ms,
200            t.len()
201        );
202
203        plot_topp3_grid(
204            "data/path_topp_plots/spline_path_topp3.png",
205            "TOPP3 Result: Spline Path (6-DOF, 50 waypoints)",
206            &t,
207            &q_t,
208            &dq_t,
209            &ddq_t,
210            &dddq_t,
211        )?;
212
213        Ok(())
214    }
215
216    fn make_s_vector(n: usize) -> DMatrix<f64> {
217        DMatrix::<f64>::from_fn(1, n, |_, j| j as f64 / (n - 1) as f64)
218    }
219
220    fn make_parametric_path_6dof() -> Result<super::Path, PathError> {
221        super::Path::from_parametric(
222            |s: Jet3| {
223                vec![
224                    sin(s),
225                    cos(s),
226                    exp(0.3 * s) - 1.0,
227                    s + 0.1 * s * s - 0.01 * s * s * s * s,
228                    sin(2.0 * s) + 0.15 * cos(3.0 * s),
229                    sin(s) * cos(s),
230                ]
231            },
232            0.0,
233            1.0,
234        )
235    }
236
237    fn make_random_waypoints(dim: usize, n_pts: usize) -> DMatrix<f64> {
238        let mut rng = rand::rng();
239        // Build a random-walk waypoint matrix: each row is one dimension,
240        // each column is a waypoint; steps are small bounded increments.
241        let mut waypoints = DMatrix::<f64>::zeros(dim, n_pts);
242        for mut row in waypoints.row_iter_mut() {
243            row[0] = rng.random_range(-1.0..1.0);
244            for j in 1..n_pts {
245                let step = rng.random_range(-0.5..0.5);
246                row[j] = row[j - 1] + step;
247            }
248        }
249        waypoints
250    }
251
252    fn setup_robot_with_constraints(
253        s: &DMatrix<f64>,
254        derivs: &PathDerivatives,
255    ) -> Result<Robot<usize>, ConstraintError> {
256        let n = s.ncols();
257        let mut robot = Robot::with_capacity(DIM, n);
258
259        let vel_max = vec![1.0; DIM];
260        let vel_min = vec![-1.0; DIM];
261        let acc_max = vec![1.0; DIM];
262        let acc_min = vec![-1.0; DIM];
263        let jerk_max = vec![5.0; DIM];
264        let jerk_min = vec![-5.0; DIM];
265        robot
266            .with_s(&s.as_view())?
267            .with_q(
268                &derivs.q.as_view(),
269                &derivs.dq.as_ref().unwrap().as_view(),
270                &derivs.ddq.as_ref().unwrap().as_view(),
271                derivs.dddq.as_ref().map(|m| m.as_view()).as_ref(),
272                0,
273            )?
274            .with_axial_velocity((vel_max.as_slice(), n), (vel_min.as_slice(), n), 0)?
275            .with_axial_acceleration((acc_max.as_slice(), n), (acc_min.as_slice(), n), 0)?
276            .with_axial_jerk((jerk_max.as_slice(), n), (jerk_min.as_slice(), n), 0)?;
277
278        Ok(robot)
279    }
280
281    struct ToppTimings {
282        topp2_ms: f64,
283        topp3_ms: f64,
284        t_motion_2: f64,
285        t_motion_3: f64,
286    }
287
288    fn solve_topp_pipeline(
289        robot: &mut Robot<usize>,
290        s_slice: &[f64],
291    ) -> Result<(Topp3Profile, ToppTimings), Box<dyn Error>> {
292        let n = s_slice.len();
293
294        let topp2_problem = Topp2ProblemBuilder::new(robot, (0, n - 1), (0.0, 0.0)).build()?;
295
296        let options = ReachSet2OptionsBuilder::new()
297            .a_cmp_abs_tol(1e-9)
298            .a_cmp_rel_tol(1e-9)
299            .lp_feas_tol(1e-9)
300            .verbosity(Verbosity::Silent)
301            .build()?;
302
303        let start = Instant::now();
304        let a_ra = topp2_ra(&topp2_problem, &options)?;
305        let topp2_ms = start.elapsed().as_secs_f64() * 1e3;
306        let (t_motion_2, _) = s_to_t_topp2(s_slice, &a_ra, 0.0)?;
307
308        robot.constraints.amax_substitute(&a_ra, 0)?;
309
310        let topp3_problem = Topp3ProblemBuilder::new(robot, 0, &a_ra, (0.0, 0.0), (0.0, 0.0))
311            .with_num_stationary_max(2)
312            .build_with_linearization()?;
313        let options_lp = ClarabelOptionsBuilder::new()
314            .allow_almost_solved(true)
315            .build()?;
316
317        let start = Instant::now();
318        let profile = topp3_lp(&topp3_problem, &options_lp)?;
319        let topp3_ms = start.elapsed().as_secs_f64() * 1e3;
320        let (t_motion_3, _) = s_to_t_topp3(s_slice, profile.as_parts(), 0.0)?;
321
322        Ok((
323            profile,
324            ToppTimings {
325                topp2_ms,
326                topp3_ms,
327                t_motion_2,
328                t_motion_3,
329            },
330        ))
331    }
332
333    #[allow(clippy::type_complexity)]
334    fn interpolate_to_time_domain(
335        path: &super::Path,
336        s_slice: &[f64],
337        profile: &Topp3Profile,
338        dt: f64,
339    ) -> Result<
340        (
341            Vec<f64>,
342            DMatrix<f64>,
343            DMatrix<f64>,
344            DMatrix<f64>,
345            DMatrix<f64>,
346            f64,
347        ),
348        Box<dyn Error>,
349    > {
350        let start = Instant::now();
351        let (t_final, t_s) = s_to_t_topp3(s_slice, profile.as_parts(), 0.0)?;
352
353        let t_sample: Vec<f64> = (0..=((t_final / dt).floor() as usize))
354            .map(|i| i as f64 * dt)
355            .collect();
356
357        let s_t = t_to_s_topp3(
358            s_slice,
359            profile.as_parts(),
360            &t_s,
361            InterpolationMode::NonUniformTimeGrid(&t_sample),
362        )?;
363
364        // Evaluate q(s(t)): only position needed, derivatives computed via finite diff
365        let n_t = t_sample.len();
366        let s_t_matrix = DMatrix::<f64>::from_row_slice(1, n_t, &s_t);
367        let q_t = path.evaluate_q(s_t_matrix.as_slice())?.q;
368
369        // Compute time-domain derivatives using finite differences
370        let mut dq_t = DMatrix::<f64>::zeros(DIM, n_t);
371        let mut ddq_t = DMatrix::<f64>::zeros(DIM, n_t);
372        let mut dddq_t = DMatrix::<f64>::zeros(DIM, n_t);
373
374        // Compute dq_t, ddq_t, dddq_t via central finite differences (forward/backward at ends).
375        finite_diff_inplace(&q_t, &mut dq_t, dt);
376        finite_diff_inplace(&dq_t, &mut ddq_t, dt);
377        finite_diff_inplace(&ddq_t, &mut dddq_t, dt);
378
379        let interp_ms = start.elapsed().as_secs_f64() * 1e3;
380
381        Ok((t_sample, q_t, dq_t, ddq_t, dddq_t, interp_ms))
382    }
383
384    fn plot_topp3_grid(
385        filename: &str,
386        title: &str,
387        t: &[f64],
388        q_t: &DMatrix<f64>,
389        dq_t: &DMatrix<f64>,
390        ddq_t: &DMatrix<f64>,
391        dddq_t: &DMatrix<f64>,
392    ) -> Result<(), Box<dyn Error>> {
393        if let Some(parent) = Path::new(filename).parent()
394            && !parent.as_os_str().is_empty()
395        {
396            create_dir_all(parent)?;
397        }
398
399        let root = BitMapBackend::new(filename, (2400, 1600)).into_drawing_area();
400        root.fill(&WHITE)?;
401
402        let areas = root.split_evenly((4, DIM));
403        let mats = [q_t, dq_t, ddq_t, dddq_t];
404        let labels = ["q(t)", "dq/dt", "d²q/dt²", "d³q/dt³"];
405        let constraints_bounds = [
406            None,
407            Some((-1.0, 1.0)),
408            Some((-1.0, 1.0)),
409            Some((-5.0, 5.0)),
410        ];
411
412        for row in 0..4 {
413            for col in 0..DIM {
414                let area = &areas[row * DIM + col];
415                let series: Vec<f64> = (0..mats[row].ncols())
416                    .map(|j| mats[row][(col, j)])
417                    .collect();
418
419                let (mut y_min, mut y_max) = min_max(&series);
420                if let Some((lb, ub)) = constraints_bounds[row] {
421                    y_min = y_min.min(lb);
422                    y_max = y_max.max(ub);
423                }
424                let pad = 0.1 * (y_max - y_min).max(0.1);
425                y_min -= pad;
426                y_max += pad;
427
428                let mut chart = ChartBuilder::on(area)
429                    .margin(10)
430                    .caption(
431                        format!("{} axis {}", labels[row], col + 1),
432                        ("sans-serif", 16),
433                    )
434                    .x_label_area_size(30)
435                    .y_label_area_size(45)
436                    .build_cartesian_2d(t[0]..t[t.len() - 1], y_min..y_max)?;
437
438                chart
439                    .configure_mesh()
440                    .x_desc(if row == 3 { "t (s)" } else { "" })
441                    .draw()?;
442
443                chart.draw_series(LineSeries::new(
444                    t.iter().zip(series.iter()).map(|(&x, &y)| (x, y)),
445                    &BLACK,
446                ))?;
447
448                if let Some((lb, ub)) = constraints_bounds[row] {
449                    chart.draw_series(LineSeries::new(
450                        vec![(t[0], lb), (t[t.len() - 1], lb)],
451                        ShapeStyle::from(&RED.mix(0.5)).stroke_width(2).filled(),
452                    ))?;
453                    chart.draw_series(LineSeries::new(
454                        vec![(t[0], ub), (t[t.len() - 1], ub)],
455                        ShapeStyle::from(&RED.mix(0.5)).stroke_width(2).filled(),
456                    ))?;
457                }
458            }
459        }
460
461        root.titled(title, ("sans-serif", 32))?;
462        root.present()?;
463        crate::verbosity_log!(crate::diag::Verbosity::Summary, "Saved plot: {filename}");
464        Ok(())
465    }
466
467    /// Compute first-order finite differences of `src` into `dst` (in-place).
468    /// Uses forward difference at index 0, backward at index n-1, central elsewhere.
469    fn finite_diff_inplace(src: &DMatrix<f64>, dst: &mut DMatrix<f64>, h: f64) {
470        let n = src.ncols();
471        if n < 2 {
472            return;
473        }
474        let inv2h = 1.0 / (2.0 * h);
475        // Iterate over each row (dimension) paired between src and dst.
476        for (src_row, mut dst_row) in src.row_iter().zip(dst.row_iter_mut()) {
477            // Forward difference at left boundary
478            dst_row[0] = (src_row[1] - src_row[0]) / h;
479            // Backward difference at right boundary
480            dst_row[n - 1] = (src_row[n - 1] - src_row[n - 2]) / h;
481            // Central differences for all interior points
482            for j in 1..n - 1 {
483                dst_row[j] = (src_row[j + 1] - src_row[j - 1]) * inv2h;
484            }
485        }
486    }
487
488    fn min_max(data: &[f64]) -> (f64, f64) {
489        data.iter()
490            .copied()
491            .filter(|v| v.is_finite())
492            .fold((f64::INFINITY, f64::NEG_INFINITY), |(mn, mx), v| {
493                (mn.min(v), mx.max(v))
494            })
495    }
496
497    #[test]
498    fn test_topp3_with_parametric_path() -> Result<(), Box<dyn Error>> {
499        crate::verbosity_log!(
500            crate::diag::Verbosity::Summary,
501            "\n=== TOPP3 with Parametric Path (6-DOF) ==="
502        );
503
504        let start = Instant::now();
505        let path = make_parametric_path_6dof()?;
506        let build_ms = start.elapsed().as_secs_f64() * 1e3;
507
508        let s = make_s_vector(N);
509        let start = Instant::now();
510        let derivs = path.evaluate_up_to_3rd(s.as_slice())?;
511        let eval_ms = start.elapsed().as_secs_f64() * 1e3;
512
513        crate::verbosity_log!(
514            crate::diag::Verbosity::Summary,
515            "[Path Build] {build_ms:.3} ms, [Path Eval] {eval_ms:.3} ms"
516        );
517
518        let mut robot = setup_robot_with_constraints(&s, &derivs)?;
519        let s_slice: Vec<f64> = (0..N).map(|j| s[(0, j)]).collect();
520        let (profile, timings) = solve_topp_pipeline(&mut robot, &s_slice)?;
521
522        crate::verbosity_log!(
523            crate::diag::Verbosity::Summary,
524            "[TOPP2-RA] {:.3} ms, t_motion = {:.3} s",
525            timings.topp2_ms,
526            timings.t_motion_2
527        );
528        crate::verbosity_log!(
529            crate::diag::Verbosity::Summary,
530            "[TOPP3-LP] {:.3} ms, t_motion = {:.3} s",
531            timings.topp3_ms,
532            timings.t_motion_3
533        );
534
535        let (t, q_t, dq_t, ddq_t, dddq_t, interp_ms) =
536            interpolate_to_time_domain(&path, &s_slice, &profile, 1e-3)?;
537        crate::verbosity_log!(
538            crate::diag::Verbosity::Summary,
539            "[Interpolation] {:.3} ms, {} samples",
540            interp_ms,
541            t.len()
542        );
543
544        plot_topp3_grid(
545            "data/path_topp_plots/parametric_path_topp3.png",
546            "TOPP3 Result: Parametric Path (6-DOF)",
547            &t,
548            &q_t,
549            &dq_t,
550            &ddq_t,
551            &dddq_t,
552        )?;
553
554        Ok(())
555    }
556}
copp\path/mod.rs

copp\path/
mod.rs
