Skip to main content

rust_robotics_planning/state_lattice/
trajectory_generator.rs

1//! Model Predictive Trajectory Generator
2//!
3//! Optimizes trajectory parameters to reach target states using Newton-Raphson method.
4//! Based on PythonRobotics implementation.
5
6use nalgebra::{Matrix3, Vector3};
7
8use super::motion_model::{normalize_angle, MotionModel};
9
10const DEFAULT_LOOKUP_TABLE_CSV: &str = include_str!("lookup_table.csv");
11
12/// Trajectory parameters: [s, km, kf]
13/// - s: arc length
14/// - km: middle curvature
15/// - kf: final curvature
16pub type TrajectoryParams = Vector3<f64>;
17pub type GeneratedTrajectory = (Vec<f64>, Vec<f64>, Vec<f64>, TrajectoryParams);
18
19/// Target state: [x, y, yaw]
20pub type TargetState = Vector3<f64>;
21
22/// Trajectory generator configuration
23#[derive(Debug, Clone)]
24pub struct TrajectoryGeneratorConfig {
25    /// Maximum optimization iterations
26    pub max_iter: usize,
27    /// Convergence threshold for cost
28    pub cost_threshold: f64,
29    /// Finite difference step for Jacobian
30    pub h: Vector3<f64>,
31    /// Initial curvature (steering at start)
32    pub k0: f64,
33}
34
35impl Default for TrajectoryGeneratorConfig {
36    fn default() -> Self {
37        Self {
38            max_iter: 100,
39            cost_threshold: 0.1,
40            h: Vector3::new(0.5, 0.02, 0.02), // [ds, dkm, dkf]
41            k0: 0.0,
42        }
43    }
44}
45
46/// Model Predictive Trajectory Generator
47pub struct TrajectoryGenerator {
48    pub(crate) motion_model: MotionModel,
49    config: TrajectoryGeneratorConfig,
50}
51
52impl TrajectoryGenerator {
53    pub fn new(motion_model: MotionModel, config: TrajectoryGeneratorConfig) -> Self {
54        Self {
55            motion_model,
56            config,
57        }
58    }
59
60    pub fn with_defaults() -> Self {
61        Self::new(
62            MotionModel::with_defaults(),
63            TrajectoryGeneratorConfig::default(),
64        )
65    }
66
67    /// Set initial curvature
68    pub fn set_k0(&mut self, k0: f64) {
69        self.config.k0 = k0;
70    }
71
72    /// Generate trajectory with given parameters
73    pub fn generate(&self, params: &TrajectoryParams) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
74        self.motion_model.generate_trajectory(
75            params[0], // s
76            self.config.k0,
77            params[1], // km
78            params[2], // kf
79        )
80    }
81
82    /// Calculate difference between current trajectory endpoint and target
83    fn calc_diff(&self, params: &TrajectoryParams, target: &TargetState) -> Vector3<f64> {
84        let (x_final, y_final, yaw_final) = self.motion_model.generate_trajectory_final_state(
85            params[0],
86            self.config.k0,
87            params[1],
88            params[2],
89        );
90
91        Vector3::new(
92            x_final - target[0],
93            y_final - target[1],
94            normalize_angle(yaw_final - target[2]),
95        )
96    }
97
98    /// Calculate cost (L2 norm of difference)
99    fn calc_cost(&self, params: &TrajectoryParams, target: &TargetState) -> f64 {
100        let diff = self.calc_diff(params, target);
101        diff.norm()
102    }
103
104    /// Calculate Jacobian matrix using finite differences
105    fn calc_jacobian(&self, params: &TrajectoryParams, target: &TargetState) -> Matrix3<f64> {
106        let h = &self.config.h;
107        let mut jacobian = Matrix3::zeros();
108        let diff_current = self.calc_diff(params, target);
109
110        for i in 0..3 {
111            let mut params_plus = *params;
112            params_plus[i] += h[i];
113            let mut params_minus = *params;
114            params_minus[i] -= h[i];
115
116            let diff_plus = self.calc_diff(&params_plus, target);
117            let diff_minus = self.calc_diff(&params_minus, target);
118
119            for j in 0..3 {
120                let delta = if i == 0 && params_minus[i] <= 0.0 {
121                    diff_plus[j] - diff_current[j]
122                } else {
123                    diff_plus[j] - diff_minus[j]
124                };
125                let denom = if i == 0 && params_minus[i] <= 0.0 {
126                    h[i]
127                } else {
128                    2.0 * h[i]
129                };
130                jacobian[(j, i)] = delta / denom;
131            }
132        }
133
134        jacobian
135    }
136
137    /// Line search to find optimal step size
138    fn line_search(
139        &self,
140        params: &TrajectoryParams,
141        dp: &Vector3<f64>,
142        target: &TargetState,
143    ) -> f64 {
144        let alphas = [1.0, 1.5];
145        let mut best_alpha = 1.0;
146        let mut min_cost = f64::MAX;
147
148        for &alpha in &alphas {
149            let new_params = params + alpha * dp;
150            if new_params[0] > 0.0 {
151                let cost = self.calc_cost(&new_params, target);
152                if cost < min_cost {
153                    min_cost = cost;
154                    best_alpha = alpha;
155                }
156            }
157        }
158
159        best_alpha
160    }
161
162    /// Optimize trajectory to reach target state
163    pub fn optimize(
164        &self,
165        target: &TargetState,
166        init_params: &TrajectoryParams,
167    ) -> Option<TrajectoryParams> {
168        let mut params = *init_params;
169
170        for _iter in 0..self.config.max_iter {
171            let cost = self.calc_cost(&params, target);
172
173            if cost < self.config.cost_threshold {
174                return Some(params);
175            }
176
177            let jacobian = self.calc_jacobian(&params, target);
178
179            let diff = self.calc_diff(&params, target);
180
181            let j_inv = jacobian.try_inverse()?;
182            let dp = -j_inv * diff;
183
184            let alpha = self.line_search(&params, &dp, target);
185            params += alpha * dp;
186
187            if params[0] < 0.1 {
188                params[0] = 0.1;
189            }
190        }
191
192        None
193    }
194
195    /// Generate optimized trajectory to reach target
196    pub fn generate_optimized(
197        &self,
198        target: &TargetState,
199        init_params: &TrajectoryParams,
200    ) -> Option<GeneratedTrajectory> {
201        let params = self.optimize(target, init_params)?;
202        let (x, y, yaw) = self.generate(&params);
203        Some((x, y, yaw, params))
204    }
205}
206
207/// Lookup table entry for trajectory generation
208#[derive(Debug, Clone)]
209pub struct LookupTableEntry {
210    pub x: f64,
211    pub y: f64,
212    pub yaw: f64,
213    pub s: f64,
214    pub km: f64,
215    pub kf: f64,
216}
217
218impl LookupTableEntry {
219    pub fn new(x: f64, y: f64, yaw: f64, s: f64, km: f64, kf: f64) -> Self {
220        Self {
221            x,
222            y,
223            yaw,
224            s,
225            km,
226            kf,
227        }
228    }
229
230    /// Get target state from entry
231    pub fn target(&self) -> TargetState {
232        Vector3::new(self.x, self.y, self.yaw)
233    }
234
235    /// Get trajectory parameters from entry
236    pub fn params(&self) -> TrajectoryParams {
237        Vector3::new(self.s, self.km, self.kf)
238    }
239
240    /// Calculate distance to a target state
241    pub fn distance_to(&self, target: &TargetState) -> f64 {
242        let dx = self.x - target[0];
243        let dy = self.y - target[1];
244        let dyaw = self.yaw - target[2];
245        (dx * dx + dy * dy + dyaw * dyaw).sqrt()
246    }
247}
248
249/// Lookup table for efficient trajectory initialization
250#[derive(Debug, Clone)]
251pub struct LookupTable {
252    entries: Vec<LookupTableEntry>,
253}
254
255impl LookupTable {
256    pub fn new() -> Self {
257        Self {
258            entries: Vec::new(),
259        }
260    }
261
262    /// Create lookup table from CSV data
263    pub fn from_csv(csv_data: &str) -> Self {
264        let mut entries = Vec::new();
265
266        for line in csv_data.lines() {
267            let line = line.trim();
268            if line.is_empty() || line.starts_with('#') || line.starts_with("x,") {
269                continue;
270            }
271
272            let parts: Vec<&str> = line.split(',').collect();
273            if parts.len() >= 6 {
274                if let (Ok(x), Ok(y), Ok(yaw), Ok(s), Ok(km), Ok(kf)) = (
275                    parts[0].trim().parse::<f64>(),
276                    parts[1].trim().parse::<f64>(),
277                    parts[2].trim().parse::<f64>(),
278                    parts[3].trim().parse::<f64>(),
279                    parts[4].trim().parse::<f64>(),
280                    parts[5].trim().parse::<f64>(),
281                ) {
282                    entries.push(LookupTableEntry::new(x, y, yaw, s, km, kf));
283                }
284            }
285        }
286
287        Self { entries }
288    }
289
290    /// Generate default lookup table with common trajectories
291    pub fn generate_default() -> Self {
292        Self::from_csv(DEFAULT_LOOKUP_TABLE_CSV)
293    }
294
295    /// Find nearest entry to target state
296    pub fn find_nearest(&self, target: &TargetState) -> Option<&LookupTableEntry> {
297        self.entries.iter().min_by(|a, b| {
298            a.distance_to(target)
299                .partial_cmp(&b.distance_to(target))
300                .unwrap_or(std::cmp::Ordering::Equal)
301        })
302    }
303
304    /// Add entry to table
305    pub fn add(&mut self, entry: LookupTableEntry) {
306        self.entries.push(entry);
307    }
308
309    /// Get number of entries
310    pub fn len(&self) -> usize {
311        self.entries.len()
312    }
313
314    /// Check if table is empty
315    pub fn is_empty(&self) -> bool {
316        self.entries.is_empty()
317    }
318
319    /// Convert to CSV string
320    pub fn to_csv(&self) -> String {
321        let mut csv = String::from("x,y,yaw,s,km,kf\n");
322        for entry in &self.entries {
323            csv.push_str(&format!(
324                "{},{},{},{},{},{}\n",
325                entry.x, entry.y, entry.yaw, entry.s, entry.km, entry.kf
326            ));
327        }
328        csv
329    }
330}
331
332impl Default for LookupTable {
333    fn default() -> Self {
334        Self::generate_default()
335    }
336}
337
338#[cfg(test)]
339mod tests {
340    use super::*;
341
342    #[test]
343    fn test_trajectory_generator_straight() {
344        let generator = TrajectoryGenerator::with_defaults();
345        let params = Vector3::new(5.0, 0.0, 0.0);
346
347        let (x, y, _yaw) = generator.generate(&params);
348
349        assert!(x.len() > 1);
350        let final_x = x.last().unwrap();
351        let final_y = y.last().unwrap();
352        assert!(*final_x > 4.0);
353        assert!(final_y.abs() < 0.1);
354    }
355
356    #[test]
357    fn test_trajectory_generator_turn() {
358        let generator = TrajectoryGenerator::with_defaults();
359        let params = Vector3::new(5.0, 0.1, 0.1);
360
361        let (x, y, _yaw) = generator.generate(&params);
362
363        assert!(x.len() > 1);
364        let final_y = y.last().unwrap();
365        assert!(*final_y > 0.0);
366    }
367
368    #[test]
369    fn test_calc_diff() {
370        let generator = TrajectoryGenerator::with_defaults();
371        let target = Vector3::new(5.0, 0.0, 0.0);
372        let params = Vector3::new(5.0, 0.0, 0.0);
373
374        let diff = generator.calc_diff(&params, &target);
375
376        assert!(diff.norm() < 1.0);
377    }
378
379    #[test]
380    fn test_optimize_straight() {
381        let generator = TrajectoryGenerator::with_defaults();
382        let target = Vector3::new(10.0, 0.0, 0.0);
383        let init_params = Vector3::new(10.0, 0.0, 0.0);
384
385        let result = generator.optimize(&target, &init_params);
386
387        assert!(result.is_some());
388        let params = result.unwrap();
389        assert!(params[0] > 0.0);
390    }
391
392    #[test]
393    fn test_optimize_turn() {
394        let generator = TrajectoryGenerator::with_defaults();
395        let target = Vector3::new(8.0, 3.0, 0.5);
396        let init_params = Vector3::new(10.0, 0.05, 0.05);
397
398        let result = generator.optimize(&target, &init_params);
399
400        if let Some(params) = result {
401            assert!(params[0] > 0.0);
402        }
403    }
404
405    #[test]
406    fn test_optimize_matches_upstream_lane_reference() {
407        let generator = TrajectoryGenerator::with_defaults();
408        let target = Vector3::new(10.0, 9.0, 0.0);
409        let init_params = Vector3::new(13.45362404707371, 0.1482242831571022, -0.5606578442626601);
410
411        let params = generator.optimize(&target, &init_params).unwrap();
412
413        assert!((params[0] - 14.806296460297).abs() < 1e-9);
414        assert!((params[1] - 0.148478839778).abs() < 1e-9);
415        assert!((params[2] - -0.57288113757).abs() < 1e-9);
416    }
417
418    #[test]
419    fn test_lookup_table_default() {
420        let table = LookupTable::generate_default();
421        assert!(!table.is_empty());
422        assert_eq!(table.len(), 81);
423    }
424
425    #[test]
426    fn test_lookup_table_default_matches_upstream_reference_rows() {
427        let table = LookupTable::generate_default();
428        let first = &table.entries[0];
429        let last = table.entries.last().unwrap();
430
431        assert!((first.x - 1.0).abs() < 1e-12);
432        assert!((first.y - 0.0).abs() < 1e-12);
433        assert!((first.yaw - 0.0).abs() < 1e-12);
434        assert!((first.s - 1.0).abs() < 1e-12);
435        assert!((first.km - 0.0).abs() < 1e-12);
436        assert!((first.kf - 0.0).abs() < 1e-12);
437
438        assert!((last.x - 24.960019173190652).abs() < 1e-12);
439        assert!((last.y - 17.98909417109214).abs() < 1e-12);
440        assert!((last.yaw - 0.011594018486178026).abs() < 1e-12);
441        assert!((last.s - 33.0995680641525).abs() < 1e-12);
442        assert!((last.km - 0.05634561447882407).abs() < 1e-12);
443        assert!((last.kf - -0.22402297280749597).abs() < 1e-12);
444    }
445
446    #[test]
447    fn test_lookup_table_find_nearest() {
448        let table = LookupTable::generate_default();
449        let target = Vector3::new(10.0, 0.0, 0.0);
450
451        let nearest = table.find_nearest(&target);
452        assert!(nearest.is_some());
453    }
454
455    #[test]
456    fn test_lookup_table_csv() {
457        let table = LookupTable::generate_default();
458        let csv = table.to_csv();
459
460        assert!(csv.contains("x,y,yaw,s,km,kf"));
461
462        let parsed = LookupTable::from_csv(&csv);
463        assert_eq!(table.len(), parsed.len());
464    }
465
466    #[test]
467    fn test_lookup_entry_distance() {
468        let entry = LookupTableEntry::new(10.0, 0.0, 0.0, 10.0, 0.0, 0.0);
469        let target = Vector3::new(10.0, 0.0, 0.0);
470
471        assert!(entry.distance_to(&target) < 0.001);
472    }
473}