rust_robotics_control/
minimum_snap_trajectory.rs

1//! Minimum-snap 3D trajectory generation for drone waypoint tracking.
2//!
3//! This module builds piecewise seventh-order polynomial segments with
4//! position, velocity, acceleration, and jerk boundary constraints. The
5//! generated trajectory can be sampled into [`DesiredState`] values and fed
6//! into the existing quadrotor PD tracker.
7
8use nalgebra::{SMatrix, SVector, Vector3};
9
10use crate::drone_3d_trajectory::{
11    simulate_desired_states, DesiredState, SimulationConfig, SimulationRecord,
12};
13
14type Matrix8 = SMatrix<f64, 8, 8>;
15type Vector8 = SVector<f64, 8>;
16
17/// Coefficients of a seventh-order polynomial:
18/// `c[0] + c[1] * t + ... + c[7] * t^7`.
19#[derive(Debug, Clone)]
20pub struct MinimumSnapCoeffs {
21    pub c: [f64; 8],
22}
23
24impl MinimumSnapCoeffs {
25    fn evaluate_derivative(&self, t: f64, order: usize) -> f64 {
26        self.c
27            .iter()
28            .enumerate()
29            .skip(order)
30            .map(|(power, coeff)| {
31                coeff * derivative_factor(power, order) * t.powi((power - order) as i32)
32            })
33            .sum()
34    }
35
36    pub fn position(&self, t: f64) -> f64 {
37        self.evaluate_derivative(t, 0)
38    }
39
40    pub fn velocity(&self, t: f64) -> f64 {
41        self.evaluate_derivative(t, 1)
42    }
43
44    pub fn acceleration(&self, t: f64) -> f64 {
45        self.evaluate_derivative(t, 2)
46    }
47
48    pub fn jerk(&self, t: f64) -> f64 {
49        self.evaluate_derivative(t, 3)
50    }
51
52    pub fn snap(&self, t: f64) -> f64 {
53        self.evaluate_derivative(t, 4)
54    }
55}
56
57fn derivative_factor(power: usize, order: usize) -> f64 {
58    ((power - order + 1)..=power).fold(1.0, |acc, value| acc * value as f64)
59}
60
61#[derive(Debug, Clone)]
62struct AxisBoundary {
63    start_pos: f64,
64    end_pos: f64,
65    start_vel: f64,
66    end_vel: f64,
67    start_acc: f64,
68    end_acc: f64,
69    start_jerk: f64,
70    end_jerk: f64,
71}
72
73/// Full boundary conditions for one 3D minimum-snap endpoint.
74#[derive(Debug, Clone)]
75pub struct MinimumSnapBoundary {
76    pub position: Vector3<f64>,
77    pub velocity: Vector3<f64>,
78    pub acceleration: Vector3<f64>,
79    pub jerk: Vector3<f64>,
80}
81
82impl MinimumSnapBoundary {
83    pub fn at_rest(position: Vector3<f64>) -> Self {
84        Self {
85            position,
86            velocity: Vector3::zeros(),
87            acceleration: Vector3::zeros(),
88            jerk: Vector3::zeros(),
89        }
90    }
91}
92
93/// A piecewise minimum-snap segment for x/y/z.
94#[derive(Debug, Clone)]
95pub struct MinimumSnapSegment {
96    pub x: MinimumSnapCoeffs,
97    pub y: MinimumSnapCoeffs,
98    pub z: MinimumSnapCoeffs,
99    pub duration: f64,
100}
101
102impl MinimumSnapSegment {
103    pub fn desired_state(&self, t: f64) -> DesiredState {
104        let t = t.clamp(0.0, self.duration);
105        DesiredState {
106            position: Vector3::new(self.x.position(t), self.y.position(t), self.z.position(t)),
107            velocity: Vector3::new(self.x.velocity(t), self.y.velocity(t), self.z.velocity(t)),
108            acceleration: Vector3::new(
109                self.x.acceleration(t),
110                self.y.acceleration(t),
111                self.z.acceleration(t),
112            ),
113            yaw: 0.0,
114        }
115    }
116
117    pub fn jerk(&self, t: f64) -> Vector3<f64> {
118        let t = t.clamp(0.0, self.duration);
119        Vector3::new(self.x.jerk(t), self.y.jerk(t), self.z.jerk(t))
120    }
121
122    pub fn snap(&self, t: f64) -> Vector3<f64> {
123        let t = t.clamp(0.0, self.duration);
124        Vector3::new(self.x.snap(t), self.y.snap(t), self.z.snap(t))
125    }
126}
127
128fn solve_minimum_snap_axis(boundary: &AxisBoundary, duration: f64) -> MinimumSnapCoeffs {
129    let t = duration;
130    let t2 = t * t;
131    let t3 = t2 * t;
132    let t4 = t3 * t;
133    let t5 = t4 * t;
134    let t6 = t5 * t;
135    let t7 = t6 * t;
136
137    #[rustfmt::skip]
138    let a = Matrix8::from_row_slice(&[
139        1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
140        1.0, t,   t2,  t3,  t4,  t5,  t6,  t7,
141        0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
142        0.0, 1.0, 2.0 * t, 3.0 * t2, 4.0 * t3, 5.0 * t4, 6.0 * t5, 7.0 * t6,
143        0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
144        0.0, 0.0, 2.0, 6.0 * t, 12.0 * t2, 20.0 * t3, 30.0 * t4, 42.0 * t5,
145        0.0, 0.0, 0.0, 6.0, 0.0, 0.0, 0.0, 0.0,
146        0.0, 0.0, 0.0, 6.0, 24.0 * t, 60.0 * t2, 120.0 * t3, 210.0 * t4,
147    ]);
148
149    let rhs = Vector8::from_row_slice(&[
150        boundary.start_pos,
151        boundary.end_pos,
152        boundary.start_vel,
153        boundary.end_vel,
154        boundary.start_acc,
155        boundary.end_acc,
156        boundary.start_jerk,
157        boundary.end_jerk,
158    ]);
159
160    let coeffs = a
161        .lu()
162        .solve(&rhs)
163        .expect("Minimum-snap matrix should be invertible");
164
165    MinimumSnapCoeffs {
166        c: [
167            coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7],
168        ],
169    }
170}
171
172pub fn generate_minimum_snap_segment(
173    start: &Vector3<f64>,
174    end: &Vector3<f64>,
175    duration: f64,
176) -> MinimumSnapSegment {
177    generate_minimum_snap_segment_full(
178        &MinimumSnapBoundary::at_rest(*start),
179        &MinimumSnapBoundary::at_rest(*end),
180        duration,
181    )
182}
183
184pub fn generate_minimum_snap_segment_full(
185    start: &MinimumSnapBoundary,
186    end: &MinimumSnapBoundary,
187    duration: f64,
188) -> MinimumSnapSegment {
189    let solve = |i: usize| {
190        solve_minimum_snap_axis(
191            &AxisBoundary {
192                start_pos: start.position[i],
193                end_pos: end.position[i],
194                start_vel: start.velocity[i],
195                end_vel: end.velocity[i],
196                start_acc: start.acceleration[i],
197                end_acc: end.acceleration[i],
198                start_jerk: start.jerk[i],
199                end_jerk: end.jerk[i],
200            },
201            duration,
202        )
203    };
204
205    MinimumSnapSegment {
206        x: solve(0),
207        y: solve(1),
208        z: solve(2),
209        duration,
210    }
211}
212
213pub fn generate_waypoint_minimum_snap_trajectory(
214    waypoints: &[Vector3<f64>],
215    segment_duration: f64,
216) -> Vec<MinimumSnapSegment> {
217    let segment_durations = vec![segment_duration; waypoints.len()];
218    generate_waypoint_minimum_snap_trajectory_with_durations(waypoints, &segment_durations)
219}
220
221pub fn generate_waypoint_minimum_snap_trajectory_with_durations(
222    waypoints: &[Vector3<f64>],
223    segment_durations: &[f64],
224) -> Vec<MinimumSnapSegment> {
225    let n = waypoints.len();
226    assert!(n >= 2, "Need at least 2 waypoints");
227    assert_eq!(
228        n,
229        segment_durations.len(),
230        "Need one segment duration per waypoint-to-waypoint segment"
231    );
232
233    (0..n)
234        .map(|i| {
235            generate_minimum_snap_segment(
236                &waypoints[i],
237                &waypoints[(i + 1) % n],
238                segment_durations[i],
239            )
240        })
241        .collect()
242}
243
244pub fn sample_minimum_snap_trajectory(
245    segments: &[MinimumSnapSegment],
246    dt: f64,
247) -> Vec<DesiredState> {
248    assert!(dt > 0.0, "dt must be positive");
249
250    let mut samples = Vec::new();
251    for segment in segments {
252        let mut t = 0.0;
253        while t <= segment.duration {
254            samples.push(segment.desired_state(t));
255            t += dt;
256        }
257    }
258    samples
259}
260
261pub fn simulate_minimum_snap_following(
262    segments: &[MinimumSnapSegment],
263    config: &SimulationConfig,
264    start_position: Vector3<f64>,
265) -> SimulationRecord {
266    let desired_states = sample_minimum_snap_trajectory(segments, config.dt);
267    simulate_desired_states(start_position, &desired_states, config)
268}
269
270#[cfg(test)]
271mod tests {
272    use super::*;
273
274    const EPSILON: f64 = 1e-9;
275
276    #[test]
277    fn test_minimum_snap_boundary_conditions() {
278        let start = Vector3::new(0.0, 0.0, 1.0);
279        let end = Vector3::new(5.0, -2.0, 3.0);
280        let seg = generate_minimum_snap_segment(&start, &end, 4.0);
281
282        assert!((seg.x.position(0.0) - start.x).abs() < EPSILON);
283        assert!((seg.y.position(4.0) - end.y).abs() < EPSILON);
284        assert!(seg.x.velocity(0.0).abs() < EPSILON);
285        assert!(seg.x.velocity(4.0).abs() < EPSILON);
286        assert!(seg.x.acceleration(0.0).abs() < EPSILON);
287        assert!(seg.x.acceleration(4.0).abs() < EPSILON);
288        assert!(seg.x.jerk(0.0).abs() < EPSILON);
289        assert!(seg.x.jerk(4.0).abs() < EPSILON);
290    }
291
292    #[test]
293    fn test_waypoint_minimum_snap_generation_with_durations() {
294        let waypoints = vec![
295            Vector3::new(0.0, 0.0, 0.0),
296            Vector3::new(4.0, 1.0, 2.0),
297            Vector3::new(2.0, 3.0, 1.0),
298        ];
299        let segments =
300            generate_waypoint_minimum_snap_trajectory_with_durations(&waypoints, &[2.0, 3.0, 4.0]);
301
302        assert_eq!(segments.len(), 3);
303        assert!((segments[0].x.position(2.0) - 4.0).abs() < EPSILON);
304        assert!((segments[1].y.position(3.0) - 3.0).abs() < EPSILON);
305        assert!((segments[2].z.position(4.0) - 0.0).abs() < EPSILON);
306    }
307
308    #[test]
309    fn test_sample_minimum_snap_trajectory() {
310        let waypoints = vec![Vector3::new(0.0, 0.0, 0.0), Vector3::new(2.0, 0.0, 0.0)];
311        let segments = generate_waypoint_minimum_snap_trajectory(&waypoints, 1.0);
312        let samples = sample_minimum_snap_trajectory(&segments, 0.5);
313
314        assert_eq!(samples.len(), 6);
315        assert!((samples[0].position.x - 0.0).abs() < EPSILON);
316        assert!((samples[2].position.x - 2.0).abs() < 1e-6);
317    }
318}
rust_robotics_control/minimum_snap_trajectory.rs

rust_robotics_control/
minimum_snap_trajectory.rs
