1use nalgebra::{Matrix3, Vector3};
7
8use super::motion_model::{normalize_angle, MotionModel};
9
10const DEFAULT_LOOKUP_TABLE_CSV: &str = include_str!("lookup_table.csv");
11
12pub type TrajectoryParams = Vector3<f64>;
17pub type GeneratedTrajectory = (Vec<f64>, Vec<f64>, Vec<f64>, TrajectoryParams);
18
19pub type TargetState = Vector3<f64>;
21
22#[derive(Debug, Clone)]
24pub struct TrajectoryGeneratorConfig {
25 pub max_iter: usize,
27 pub cost_threshold: f64,
29 pub h: Vector3<f64>,
31 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), k0: 0.0,
42 }
43 }
44}
45
46pub 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 pub fn set_k0(&mut self, k0: f64) {
69 self.config.k0 = k0;
70 }
71
72 pub fn generate(&self, params: &TrajectoryParams) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
74 self.motion_model.generate_trajectory(
75 params[0], self.config.k0,
77 params[1], params[2], )
80 }
81
82 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 fn calc_cost(&self, params: &TrajectoryParams, target: &TargetState) -> f64 {
100 let diff = self.calc_diff(params, target);
101 diff.norm()
102 }
103
104 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(¶ms_plus, target);
117 let diff_minus = self.calc_diff(¶ms_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 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 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(¶ms, target);
172
173 if cost < self.config.cost_threshold {
174 return Some(params);
175 }
176
177 let jacobian = self.calc_jacobian(¶ms, target);
178
179 let diff = self.calc_diff(¶ms, target);
180
181 let j_inv = jacobian.try_inverse()?;
182 let dp = -j_inv * diff;
183
184 let alpha = self.line_search(¶ms, &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 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(¶ms);
203 Some((x, y, yaw, params))
204 }
205}
206
207#[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 pub fn target(&self) -> TargetState {
232 Vector3::new(self.x, self.y, self.yaw)
233 }
234
235 pub fn params(&self) -> TrajectoryParams {
237 Vector3::new(self.s, self.km, self.kf)
238 }
239
240 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#[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 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 pub fn generate_default() -> Self {
292 Self::from_csv(DEFAULT_LOOKUP_TABLE_CSV)
293 }
294
295 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 pub fn add(&mut self, entry: LookupTableEntry) {
306 self.entries.push(entry);
307 }
308
309 pub fn len(&self) -> usize {
311 self.entries.len()
312 }
313
314 pub fn is_empty(&self) -> bool {
316 self.entries.is_empty()
317 }
318
319 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(¶ms);
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(¶ms);
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(¶ms, &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}