1#![allow(dead_code)]
2
3use nalgebra::{Matrix2, Matrix4, Matrix4x2, Vector2, Vector4};
15use std::f64::consts::PI;
16
17const WB: f64 = 2.5; const MAX_STEER: f64 = 45.0 * PI / 180.0; const MAX_DSTEER: f64 = 30.0 * PI / 180.0; const MAX_SPEED: f64 = 55.0 / 3.6; const MIN_SPEED: f64 = -20.0 / 3.6; const MAX_ACCEL: f64 = 1.0; const T: usize = 5; const DT: f64 = 0.2; const TARGET_SPEED: f64 = 10.0 / 3.6; const GOAL_DIS: f64 = 1.5; const STOP_SPEED: f64 = 0.5 / 3.6; const MAX_ITER: usize = 3; const DU_TH: f64 = 0.1; const N_IND_SEARCH: usize = 10; const PATH_RESOLUTION: f64 = 0.5; const UPSTREAM_SWITCH_BACK_TICK: f64 = 1.0; const MAX_SIM_STEPS: usize = 2000;
37
38const QP_MAX_ITERS: usize = 200;
40const LINE_SEARCH_ITERS: usize = 16;
41const LINE_SEARCH_INITIAL_STEP: f64 = 0.25;
42const GRAD_TOL: f64 = 1.0e-5;
43const COST_TOL: f64 = 1.0e-7;
44
45const Q: [f64; 4] = [1.0, 1.0, 0.5, 0.5];
47const QF: [f64; 4] = Q;
48const R: [f64; 2] = [0.01, 0.01];
49const RD: [f64; 2] = [0.01, 1.0];
50
51#[derive(Clone, Copy, Debug)]
53pub struct State {
54 pub x: f64,
55 pub y: f64,
56 pub v: f64,
57 pub yaw: f64,
58}
59
60impl State {
61 pub fn new(x: f64, y: f64, v: f64, yaw: f64) -> Self {
62 Self { x, y, v, yaw }
63 }
64
65 pub fn to_vector(self) -> Vector4<f64> {
66 Vector4::new(self.x, self.y, self.v, self.yaw)
67 }
68
69 pub fn update(&mut self, accel: f64, steer: f64) {
70 let steer = steer.clamp(-MAX_STEER, MAX_STEER);
71
72 self.x += self.v * self.yaw.cos() * DT;
73 self.y += self.v * self.yaw.sin() * DT;
74 self.yaw += self.v / WB * steer.tan() * DT;
75 self.yaw = normalize_angle(self.yaw);
76 self.v += accel * DT;
77 self.v = self.v.clamp(MIN_SPEED, MAX_SPEED);
78 }
79}
80
81#[derive(Debug)]
82pub struct MpcResult {
83 pub controls: Vec<Vector2<f64>>,
84 pub predicted: Vec<Vector4<f64>>,
85}
86
87#[derive(Debug)]
88pub struct SimulationResult {
89 pub cx: Vec<f64>,
90 pub cy: Vec<f64>,
91 pub hist_x: Vec<f64>,
92 pub hist_y: Vec<f64>,
93 pub predicted_x: Vec<f64>,
94 pub predicted_y: Vec<f64>,
95 pub goal: (f64, f64),
96 pub reached_goal: bool,
97 pub target_index: usize,
98 pub final_index: usize,
99 pub final_state: State,
100}
101
102pub use rust_robotics_core::normalize_angle;
103
104fn angle_diff(a: f64, b: f64) -> f64 {
105 normalize_angle(a - b)
106}
107
108fn state_error(state: Vector4<f64>, reference: Vector4<f64>) -> Vector4<f64> {
109 let mut error = state - reference;
110 error[3] = angle_diff(state[3], reference[3]);
111 error
112}
113
114fn control_cost_weight() -> Matrix2<f64> {
115 Matrix2::from_diagonal(&Vector2::new(R[0], R[1]))
116}
117
118fn control_rate_weight() -> Matrix2<f64> {
119 Matrix2::from_diagonal(&Vector2::new(RD[0], RD[1]))
120}
121
122fn state_cost_weight() -> Matrix4<f64> {
123 Matrix4::from_diagonal(&Vector4::new(Q[0], Q[1], Q[2], Q[3]))
124}
125
126fn terminal_cost_weight() -> Matrix4<f64> {
127 Matrix4::from_diagonal(&Vector4::new(QF[0], QF[1], QF[2], QF[3]))
128}
129
130fn get_linear_model_matrix(
131 v: f64,
132 yaw: f64,
133 steer: f64,
134) -> (Matrix4<f64>, Matrix4x2<f64>, Vector4<f64>) {
135 let a = Matrix4::new(
136 1.0,
137 0.0,
138 DT * yaw.cos(),
139 -DT * v * yaw.sin(),
140 0.0,
141 1.0,
142 DT * yaw.sin(),
143 DT * v * yaw.cos(),
144 0.0,
145 0.0,
146 1.0,
147 0.0,
148 0.0,
149 0.0,
150 DT * steer.tan() / WB,
151 1.0,
152 );
153
154 let b = Matrix4x2::new(
155 0.0,
156 0.0,
157 0.0,
158 0.0,
159 DT,
160 0.0,
161 0.0,
162 DT * v / (WB * steer.cos().powi(2)),
163 );
164
165 let c = Vector4::new(
166 DT * v * yaw.sin() * yaw,
167 -DT * v * yaw.cos() * yaw,
168 0.0,
169 -DT * v * steer / (WB * steer.cos().powi(2)),
170 );
171
172 (a, b, c)
173}
174
175struct CubicSpline1D {
177 x: Vec<f64>,
178 a: Vec<f64>,
179 b: Vec<f64>,
180 c: Vec<f64>,
181 d: Vec<f64>,
182}
183
184impl CubicSpline1D {
185 fn new(x: &[f64], y: &[f64]) -> Self {
186 let n = x.len();
187 let a = y.to_vec();
188 let mut b = vec![0.0; n];
189 let mut c = vec![0.0; n];
190 let mut d = vec![0.0; n];
191
192 let h: Vec<f64> = (0..n - 1).map(|i| x[i + 1] - x[i]).collect();
193
194 let mut alpha = vec![0.0; n];
195 for i in 1..n - 1 {
196 alpha[i] = 3.0 / h[i] * (a[i + 1] - a[i]) - 3.0 / h[i - 1] * (a[i] - a[i - 1]);
197 }
198
199 let mut l = vec![1.0; n];
200 let mut mu = vec![0.0; n];
201 let mut z = vec![0.0; n];
202
203 for i in 1..n - 1 {
204 l[i] = 2.0 * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
205 mu[i] = h[i] / l[i];
206 z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i];
207 }
208
209 for j in (0..n - 1).rev() {
210 c[j] = z[j] - mu[j] * c[j + 1];
211 b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2.0 * c[j]) / 3.0;
212 d[j] = (c[j + 1] - c[j]) / (3.0 * h[j]);
213 }
214
215 Self {
216 x: x.to_vec(),
217 a,
218 b,
219 c,
220 d,
221 }
222 }
223
224 fn calc(&self, t: f64) -> f64 {
225 let i = self.search_index(t);
226 let dx = t - self.x[i];
227 self.a[i] + self.b[i] * dx + self.c[i] * dx.powi(2) + self.d[i] * dx.powi(3)
228 }
229
230 fn calc_d(&self, t: f64) -> f64 {
231 let i = self.search_index(t);
232 let dx = t - self.x[i];
233 self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx.powi(2)
234 }
235
236 fn search_index(&self, t: f64) -> usize {
237 for i in 0..self.x.len() - 1 {
238 if self.x[i] <= t && t < self.x[i + 1] {
239 return i;
240 }
241 }
242 self.x.len() - 2
243 }
244}
245
246struct CubicSpline2D {
248 s: Vec<f64>,
249 sx: CubicSpline1D,
250 sy: CubicSpline1D,
251}
252
253impl CubicSpline2D {
254 fn new(x: &[f64], y: &[f64]) -> Self {
255 let mut s = vec![0.0];
256 for i in 1..x.len() {
257 let ds = ((x[i] - x[i - 1]).powi(2) + (y[i] - y[i - 1]).powi(2)).sqrt();
258 s.push(s[i - 1] + ds);
259 }
260
261 let sx = CubicSpline1D::new(&s, x);
262 let sy = CubicSpline1D::new(&s, y);
263
264 Self { s, sx, sy }
265 }
266
267 fn calc_position(&self, s: f64) -> (f64, f64) {
268 (self.sx.calc(s), self.sy.calc(s))
269 }
270
271 fn calc_yaw(&self, s: f64) -> f64 {
272 let dx = self.sx.calc_d(s);
273 let dy = self.sy.calc_d(s);
274 dy.atan2(dx)
275 }
276}
277
278fn smooth_yaw(yaw: &mut [f64]) {
279 for i in 0..yaw.len().saturating_sub(1) {
280 let mut dyaw = yaw[i + 1] - yaw[i];
281 while dyaw >= PI / 2.0 {
282 yaw[i + 1] -= 2.0 * PI;
283 dyaw = yaw[i + 1] - yaw[i];
284 }
285 while dyaw <= -PI / 2.0 {
286 yaw[i + 1] += 2.0 * PI;
287 dyaw = yaw[i + 1] - yaw[i];
288 }
289 }
290}
291
292pub fn calc_speed_profile(cx: &[f64], cy: &[f64], cyaw: &[f64], target_speed: f64) -> Vec<f64> {
293 let mut speed_profile = vec![target_speed; cx.len()];
294 let mut direction = 1.0;
295
296 for i in 0..cx.len().saturating_sub(1) {
297 let dx = cx[i + 1] - cx[i];
298 let dy = cy[i + 1] - cy[i];
299 let move_direction = dy.atan2(dx);
300
301 if dx.abs() > f64::EPSILON && dy.abs() > f64::EPSILON {
302 let dangle = angle_diff(move_direction, cyaw[i]).abs();
303 direction = if dangle >= PI / 4.0 { -1.0 } else { 1.0 };
304 }
305
306 speed_profile[i] = direction * target_speed;
307 }
308
309 if let Some(last) = speed_profile.last_mut() {
310 *last = 0.0;
311 }
312
313 speed_profile
314}
315
316fn calc_nearest_index(
317 state: &State,
318 cx: &[f64],
319 cy: &[f64],
320 cyaw: &[f64],
321 pind: usize,
322) -> (usize, f64) {
323 let start = pind.min(cx.len().saturating_sub(1));
324 let end = (start + N_IND_SEARCH).min(cx.len());
325
326 let mut best_index = start;
327 let mut best_distance_sq = f64::INFINITY;
328
329 for i in start..end {
330 let dx = state.x - cx[i];
331 let dy = state.y - cy[i];
332 let distance_sq = dx * dx + dy * dy;
333 if distance_sq < best_distance_sq {
334 best_distance_sq = distance_sq;
335 best_index = i;
336 }
337 }
338
339 let mut best_distance = best_distance_sq.sqrt();
340 let dxl = cx[best_index] - state.x;
341 let dyl = cy[best_index] - state.y;
342 let angle = angle_diff(cyaw[best_index], dyl.atan2(dxl));
343 if angle < 0.0 {
344 best_distance *= -1.0;
345 }
346
347 (best_index, best_distance)
348}
349
350fn calc_ref_trajectory(
351 state: &State,
352 cx: &[f64],
353 cy: &[f64],
354 cyaw: &[f64],
355 speed_profile: &[f64],
356 course_tick: f64,
357 pind: usize,
358) -> (Vec<Vector4<f64>>, usize) {
359 let ncourse = cx.len();
360 let (mut ind, _) = calc_nearest_index(state, cx, cy, cyaw, pind);
361 if pind >= ind {
362 ind = pind;
363 }
364
365 let mut xref = vec![Vector4::zeros(); T + 1];
366 xref[0] = Vector4::new(cx[ind], cy[ind], speed_profile[ind], cyaw[ind]);
367
368 let mut travel = 0.0;
369 for point in xref.iter_mut().take(T + 1).skip(1) {
370 travel += state.v.abs() * DT;
371 let dind = (travel / course_tick).round() as usize;
372 let index = (ind + dind).min(ncourse - 1);
373 *point = Vector4::new(cx[index], cy[index], speed_profile[index], cyaw[index]);
374 }
375
376 (xref, ind)
377}
378
379fn predict_motion(state: State, controls: &[Vector2<f64>]) -> Vec<Vector4<f64>> {
380 let mut predicted = vec![Vector4::zeros(); T + 1];
381 predicted[0] = state.to_vector();
382
383 let mut current = state;
384 for (i, control) in controls.iter().enumerate().take(T) {
385 current.update(control[0], control[1]);
386 predicted[i + 1] = current.to_vector();
387 }
388
389 predicted
390}
391
392fn apply_control_constraints(controls: &mut [Vector2<f64>]) {
393 for i in 0..controls.len() {
394 controls[i][0] = controls[i][0].clamp(-MAX_ACCEL, MAX_ACCEL);
395 controls[i][1] = controls[i][1].clamp(-MAX_STEER, MAX_STEER);
396
397 if i > 0 {
398 let delta = controls[i][1] - controls[i - 1][1];
399 let max_delta = MAX_DSTEER * DT;
400 if delta.abs() > max_delta {
401 controls[i][1] = controls[i - 1][1] + max_delta * delta.signum();
402 }
403 }
404 }
405}
406
407type LinearizedRollout = (
408 Vec<Vector4<f64>>,
409 Vec<Matrix4<f64>>,
410 Vec<Matrix4x2<f64>>,
411 Vec<Vector4<f64>>,
412);
413
414fn linearized_rollout(
415 x0: &State,
416 xbar: &[Vector4<f64>],
417 controls: &[Vector2<f64>],
418) -> LinearizedRollout {
419 let mut x = vec![Vector4::zeros(); T + 1];
420 let mut a_seq = Vec::with_capacity(T);
421 let mut b_seq = Vec::with_capacity(T);
422 let mut c_seq = Vec::with_capacity(T);
423
424 x[0] = x0.to_vector();
425 for t in 0..T {
426 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
427 let mut next = a * x[t] + b * controls[t] + c;
428 next[3] = normalize_angle(next[3]);
429
430 a_seq.push(a);
431 b_seq.push(b);
432 c_seq.push(c);
433 x[t + 1] = next;
434 }
435
436 (x, a_seq, b_seq, c_seq)
437}
438
439fn compute_cost(x: &[Vector4<f64>], xref: &[Vector4<f64>], controls: &[Vector2<f64>]) -> f64 {
440 let q = state_cost_weight();
441 let qf = terminal_cost_weight();
442 let r = control_cost_weight();
443 let rd = control_rate_weight();
444
445 let mut cost = 0.0;
446 for t in 0..T {
447 cost += controls[t].dot(&(r * controls[t]));
448 if t != 0 {
449 let err = state_error(x[t], xref[t]);
450 cost += err.dot(&(q * err));
451 }
452 if t < T - 1 {
453 let du = controls[t + 1] - controls[t];
454 cost += du.dot(&(rd * du));
455 }
456 }
457
458 let terminal_error = state_error(x[T], xref[T]);
459 cost + terminal_error.dot(&(qf * terminal_error))
460}
461
462#[allow(clippy::needless_range_loop)]
463fn solve_qp_clarabel(
464 xref: &[Vector4<f64>],
465 xbar: &[Vector4<f64>],
466 state: &State,
467) -> Option<Vec<Vector2<f64>>> {
468 use clarabel::algebra::*;
469 use clarabel::solver::*;
470
471 let nu = 2;
472 let nx = 4;
473 let n_u = nu * T;
474 let n_x = nx * (T + 1);
475 let n = n_u + n_x;
476
477 let u_idx = |t: usize, k: usize| -> usize { t * nu + k };
478 let x_idx = |t: usize, k: usize| -> usize { n_u + t * nx + k };
479
480 let mut p_rows = Vec::new();
482 let mut p_cols = Vec::new();
483 let mut p_vals = Vec::new();
484
485 for t in 0..T {
486 for k in 0..nu {
487 let idx = u_idx(t, k);
488 let mut val = R[k];
489 if t > 0 {
490 val += RD[k];
491 }
492 if t < T - 1 {
493 val += RD[k];
494 }
495 p_rows.push(idx);
496 p_cols.push(idx);
497 p_vals.push(val * 2.0);
498 }
499 if t < T - 1 {
500 for k in 0..nu {
501 let i = u_idx(t, k);
502 let j = u_idx(t + 1, k);
503 p_rows.push(i);
504 p_cols.push(j);
505 p_vals.push(-RD[k] * 2.0);
506 }
507 }
508 }
509
510 for t in 1..=T {
511 let w = if t == T { QF } else { Q };
512 for k in 0..nx {
513 let idx = x_idx(t, k);
514 p_rows.push(idx);
515 p_cols.push(idx);
516 p_vals.push(w[k] * 2.0);
517 }
518 }
519
520 let p = CscMatrix::new_from_triplets(n, n, p_rows, p_cols, p_vals);
521
522 let mut q_vec = vec![0.0; n];
524 for t in 1..=T {
525 let w = if t == T { QF } else { Q };
526 for k in 0..nx {
527 q_vec[x_idx(t, k)] = -w[k] * 2.0 * xref[t][k];
528 }
529 }
530
531 let n_eq = nx * (T + 1);
533 let n_steer_rate = T - 1;
534 let n_u_box = 2 * nu * T;
535 let n_steer_rate_box = 2 * n_steer_rate;
536 let n_v_box = 2 * (T + 1);
537 let n_ineq = n_u_box + n_steer_rate_box + n_v_box;
538
539 let mut a_rows = Vec::new();
540 let mut a_cols = Vec::new();
541 let mut a_vals = Vec::new();
542 let mut b_vec = vec![0.0; n_eq + n_ineq];
543
544 let x0 = state.to_vector();
545 for k in 0..nx {
546 a_rows.push(k);
547 a_cols.push(x_idx(0, k));
548 a_vals.push(1.0);
549 b_vec[k] = x0[k];
550 }
551
552 for t in 0..T {
553 let (a_mat, b_mat, c_vec) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
554 let base_row = nx * (t + 1);
555
556 for i in 0..nx {
557 let row = base_row + i;
558 a_rows.push(row);
559 a_cols.push(x_idx(t + 1, i));
560 a_vals.push(-1.0);
561 for j in 0..nx {
562 let val = a_mat[(i, j)];
563 if val.abs() > 1e-15 {
564 a_rows.push(row);
565 a_cols.push(x_idx(t, j));
566 a_vals.push(val);
567 }
568 }
569 for j in 0..nu {
570 let val = b_mat[(i, j)];
571 if val.abs() > 1e-15 {
572 a_rows.push(row);
573 a_cols.push(u_idx(t, j));
574 a_vals.push(val);
575 }
576 }
577 b_vec[row] = -c_vec[i];
578 }
579 }
580
581 let ineq_offset = n_eq;
582 let mut ineq_row = 0;
583
584 let u_bounds = [MAX_ACCEL, MAX_STEER];
585 for t in 0..T {
586 for (k, &bound) in u_bounds.iter().enumerate() {
587 let row = ineq_offset + ineq_row;
588 a_rows.push(row);
589 a_cols.push(u_idx(t, k));
590 a_vals.push(1.0);
591 b_vec[row] = bound;
592 ineq_row += 1;
593
594 let row = ineq_offset + ineq_row;
595 a_rows.push(row);
596 a_cols.push(u_idx(t, k));
597 a_vals.push(-1.0);
598 b_vec[row] = bound;
599 ineq_row += 1;
600 }
601 }
602
603 let max_steer_delta = MAX_DSTEER * DT;
604 for t in 0..(T - 1) {
605 let row = ineq_offset + ineq_row;
606 a_rows.push(row);
607 a_cols.push(u_idx(t + 1, 1));
608 a_vals.push(1.0);
609 a_rows.push(row);
610 a_cols.push(u_idx(t, 1));
611 a_vals.push(-1.0);
612 b_vec[row] = max_steer_delta;
613 ineq_row += 1;
614
615 let row = ineq_offset + ineq_row;
616 a_rows.push(row);
617 a_cols.push(u_idx(t + 1, 1));
618 a_vals.push(-1.0);
619 a_rows.push(row);
620 a_cols.push(u_idx(t, 1));
621 a_vals.push(1.0);
622 b_vec[row] = max_steer_delta;
623 ineq_row += 1;
624 }
625
626 for t in 0..=T {
627 let row = ineq_offset + ineq_row;
628 a_rows.push(row);
629 a_cols.push(x_idx(t, 2));
630 a_vals.push(1.0);
631 b_vec[row] = MAX_SPEED;
632 ineq_row += 1;
633
634 let row = ineq_offset + ineq_row;
635 a_rows.push(row);
636 a_cols.push(x_idx(t, 2));
637 a_vals.push(-1.0);
638 b_vec[row] = -MIN_SPEED;
639 ineq_row += 1;
640 }
641
642 let m = n_eq + n_ineq;
643 let a_csc = CscMatrix::new_from_triplets(m, n, a_rows, a_cols, a_vals);
644
645 let cones = vec![
646 SupportedConeT::ZeroConeT(n_eq),
647 SupportedConeT::NonnegativeConeT(n_ineq),
648 ];
649
650 let settings = DefaultSettingsBuilder::default()
651 .verbose(false)
652 .build()
653 .expect("DefaultSettingsBuilder with defaults should not fail");
655
656 let mut solver = DefaultSolver::new(&p, &q_vec, &a_csc, &b_vec, &cones, settings).ok()?;
657 solver.solve();
658
659 if solver.solution.status != SolverStatus::Solved {
660 return None;
661 }
662
663 let z = &solver.solution.x;
664 let controls: Vec<Vector2<f64>> = (0..T)
665 .map(|t| Vector2::new(z[u_idx(t, 0)], z[u_idx(t, 1)]))
666 .collect();
667
668 Some(controls)
669}
670
671fn optimize_linearized_controls(
672 xref: &[Vector4<f64>],
673 xbar: &[Vector4<f64>],
674 state: &State,
675 initial_controls: &[Vector2<f64>],
676) -> Vec<Vector2<f64>> {
677 let pg_controls = optimize_linearized_controls_pg(xref, xbar, state, initial_controls);
678
679 let is_reverse = state.v < -0.1 || xref.iter().any(|x| x[2] < -0.1);
683 if is_reverse {
684 if let Some(qp_controls) = solve_qp_clarabel(xref, xbar, state) {
685 let (qp_x, _, _, _) = linearized_rollout(state, xbar, &qp_controls);
686 let qp_cost = compute_cost(&qp_x, xref, &qp_controls);
687
688 let (pg_x, _, _, _) = linearized_rollout(state, xbar, &pg_controls);
689 let pg_cost = compute_cost(&pg_x, xref, &pg_controls);
690
691 if qp_cost <= pg_cost {
692 return qp_controls;
693 }
694 }
695 }
696
697 pg_controls
698}
699
700fn optimize_linearized_controls_pg(
701 xref: &[Vector4<f64>],
702 xbar: &[Vector4<f64>],
703 state: &State,
704 initial_controls: &[Vector2<f64>],
705) -> Vec<Vector2<f64>> {
706 let q = state_cost_weight();
707 let qf = terminal_cost_weight();
708 let r = control_cost_weight();
709 let rd = control_rate_weight();
710
711 let mut controls = initial_controls.to_vec();
712 controls.resize(T, Vector2::zeros());
713 apply_control_constraints(&mut controls);
714
715 let mut previous_cost = f64::INFINITY;
716
717 for _ in 0..QP_MAX_ITERS {
718 let (x, a_seq, b_seq, _) = linearized_rollout(state, xbar, &controls);
719 let current_cost = compute_cost(&x, xref, &controls);
720
721 let mut gradients = [Vector2::zeros(); T];
722 let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
723
724 for t in (0..T).rev() {
725 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
726
727 if t > 0 {
728 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
729 }
730 if t < T - 1 {
731 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
732 }
733
734 gradients[t] = grad;
735
736 let lx = if t == 0 {
737 Vector4::zeros()
738 } else {
739 (q * state_error(x[t], xref[t])) * 2.0
740 };
741 lambda = lx + a_seq[t].transpose() * lambda;
742 }
743
744 let gradient_norm = gradients
745 .iter()
746 .map(Vector2::norm_squared)
747 .sum::<f64>()
748 .sqrt();
749
750 if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
751 break;
752 }
753
754 previous_cost = current_cost;
755 let current_controls = controls.clone();
756 let mut best_candidate_controls = None;
757 let mut best_candidate_cost = current_cost;
758
759 let mut step = LINE_SEARCH_INITIAL_STEP;
760 for &large_step in &[1.0, 0.5] {
761 let mut candidate = current_controls.clone();
762 for t in 0..T {
763 candidate[t] -= gradients[t] * large_step;
764 }
765 apply_control_constraints(&mut candidate);
766
767 let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
768 let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
769 if candidate_cost < best_candidate_cost {
770 best_candidate_cost = candidate_cost;
771 best_candidate_controls = Some(candidate);
772 }
773 }
774
775 for _ in 0..LINE_SEARCH_ITERS {
776 let mut candidate = current_controls.clone();
777 for t in 0..T {
778 candidate[t] -= gradients[t] * step;
779 }
780 apply_control_constraints(&mut candidate);
781
782 let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
783 let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
784 if candidate_cost < best_candidate_cost {
785 best_candidate_cost = candidate_cost;
786 best_candidate_controls = Some(candidate);
787 }
788
789 step *= 0.5;
790 }
791
792 if let Some(candidate) = best_candidate_controls {
793 controls = candidate;
794 } else {
795 break;
796 }
797 }
798
799 controls
800}
801
802pub fn iterative_linear_mpc_control(
803 xref: &[Vector4<f64>],
804 state: &State,
805 warm_start: &[Vector2<f64>],
806) -> MpcResult {
807 let mut controls = warm_start.to_vec();
808 controls.resize(T, Vector2::zeros());
809 apply_control_constraints(&mut controls);
810
811 let mut predicted = predict_motion(*state, &controls);
812 for _ in 0..MAX_ITER {
813 let previous_controls = controls.clone();
814 controls = optimize_linearized_controls(xref, &predicted, state, &controls);
815 predicted = predict_motion(*state, &controls);
816
817 let du = controls
818 .iter()
819 .zip(previous_controls.iter())
820 .map(|(current, previous)| {
821 (current[0] - previous[0]).abs() + (current[1] - previous[1]).abs()
822 })
823 .sum::<f64>();
824
825 if du <= DU_TH {
826 break;
827 }
828 }
829
830 MpcResult {
831 controls,
832 predicted,
833 }
834}
835
836fn next_warm_start(controls: &[Vector2<f64>]) -> Vec<Vector2<f64>> {
837 let mut warm_start = controls[1..].to_vec();
838 warm_start.push(controls.last().copied().unwrap_or(Vector2::zeros()));
839 warm_start
840}
841
842pub fn check_goal(
843 state: &State,
844 goal: (f64, f64),
845 target_index: usize,
846 final_index: usize,
847) -> bool {
848 let dx = state.x - goal.0;
849 let dy = state.y - goal.1;
850 let distance = (dx * dx + dy * dy).sqrt();
851
852 let near_goal = distance <= GOAL_DIS;
853 let near_end = final_index.abs_diff(target_index) < 5;
854 let stopped = state.v.abs() <= STOP_SPEED;
855
856 near_goal && near_end && stopped
857}
858
859fn sample_reference_course(
860 ax: &[f64],
861 ay: &[f64],
862 course_tick: f64,
863) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
864 let csp = CubicSpline2D::new(ax, ay);
865 let s_max = *csp.s.last().unwrap_or(&0.0);
866
867 let mut cx = Vec::new();
868 let mut cy = Vec::new();
869 let mut cyaw = Vec::new();
870
871 let mut s = 0.0;
872 while s <= s_max {
873 let (x, y) = csp.calc_position(s);
874 cx.push(x);
875 cy.push(y);
876 cyaw.push(csp.calc_yaw(s));
877 s += course_tick;
878 }
879
880 if cx.is_empty() || cy.is_empty() || cyaw.is_empty() {
881 cx.push(ax[0]);
882 cy.push(ay[0]);
883 cyaw.push(0.0);
884 }
885
886 (cx, cy, cyaw)
887}
888
889pub fn generate_reference_course(ax: &[f64], ay: &[f64]) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
890 let (cx, cy, mut cyaw) = sample_reference_course(ax, ay, PATH_RESOLUTION);
891
892 smooth_yaw(&mut cyaw);
893 (cx, cy, cyaw)
894}
895
896fn generate_switch_back_course() -> (Vec<f64>, Vec<f64>, Vec<f64>) {
897 let (mut cx, mut cy, mut cyaw) = sample_reference_course(
898 &[0.0, 30.0, 6.0, 20.0, 35.0],
899 &[0.0, 0.0, 20.0, 35.0, 20.0],
900 UPSTREAM_SWITCH_BACK_TICK,
901 );
902 let (cx2, cy2, mut cyaw2) = sample_reference_course(
903 &[35.0, 10.0, 0.0, 0.0],
904 &[20.0, 30.0, 5.0, 0.0],
905 UPSTREAM_SWITCH_BACK_TICK,
906 );
907
908 for yaw in &mut cyaw2 {
909 *yaw -= PI;
910 }
911
912 cx.extend(cx2);
913 cy.extend(cy2);
914 cyaw.extend(cyaw2);
915 smooth_yaw(&mut cyaw);
916
917 (cx, cy, cyaw)
918}
919
920pub fn run_mpc_simulation() -> SimulationResult {
921 let (cx, cy, cyaw) = generate_switch_back_course();
922 run_mpc_simulation_with_reference(cx, cy, cyaw, UPSTREAM_SWITCH_BACK_TICK, MAX_SIM_STEPS)
923}
924
925fn run_mpc_simulation_with_course(
926 ax: &[f64],
927 ay: &[f64],
928 max_sim_steps: usize,
929) -> SimulationResult {
930 let (cx, cy, cyaw) = generate_reference_course(ax, ay);
931 run_mpc_simulation_with_reference(cx, cy, cyaw, PATH_RESOLUTION, max_sim_steps)
932}
933
934fn run_mpc_simulation_with_reference(
935 cx: Vec<f64>,
936 cy: Vec<f64>,
937 cyaw: Vec<f64>,
938 course_tick: f64,
939 max_sim_steps: usize,
940) -> SimulationResult {
941 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
942
943 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
944 if state.yaw - cyaw[0] >= PI {
945 state.yaw -= 2.0 * PI;
946 } else if state.yaw - cyaw[0] <= -PI {
947 state.yaw += 2.0 * PI;
948 }
949
950 let mut target_index = 0;
951 let final_index = cx.len() - 1;
952 let goal = (cx[final_index], cy[final_index]);
953
954 let mut warm_start = vec![Vector2::zeros(); T];
955
956 let mut hist_x = vec![state.x];
957 let mut hist_y = vec![state.y];
958 let mut predicted = vec![state.to_vector()];
959 let mut reached_goal = false;
960
961 for _ in 0..max_sim_steps {
962 let (xref, new_index) = calc_ref_trajectory(
963 &state,
964 &cx,
965 &cy,
966 &cyaw,
967 &speed_profile,
968 course_tick,
969 target_index,
970 );
971 target_index = new_index;
972
973 let mpc_result = iterative_linear_mpc_control(&xref, &state, &warm_start);
974 let accel = mpc_result.controls[0][0];
975 let steer = mpc_result.controls[0][1];
976
977 state.update(accel, steer);
978 hist_x.push(state.x);
979 hist_y.push(state.y);
980 predicted = mpc_result.predicted;
981
982 warm_start = next_warm_start(&mpc_result.controls);
983
984 if check_goal(&state, goal, target_index, final_index) {
985 reached_goal = true;
986 break;
987 }
988 }
989
990 SimulationResult {
991 cx,
992 cy,
993 hist_x,
994 hist_y,
995 predicted_x: predicted.iter().map(|p| p[0]).collect(),
996 predicted_y: predicted.iter().map(|p| p[1]).collect(),
997 goal,
998 reached_goal,
999 target_index,
1000 final_index,
1001 final_state: state,
1002 }
1003}
1004
1005#[cfg(test)]
1006mod tests {
1007 use super::*;
1008 use std::collections::BTreeMap;
1009
1010 fn optimize_linearized_controls_original(
1013 xref: &[Vector4<f64>],
1014 xbar: &[Vector4<f64>],
1015 state: &State,
1016 initial_controls: &[Vector2<f64>],
1017 ) -> Vec<Vector2<f64>> {
1018 let q = state_cost_weight();
1019 let qf = terminal_cost_weight();
1020 let r = control_cost_weight();
1021 let rd = control_rate_weight();
1022
1023 let mut controls = initial_controls.to_vec();
1024 controls.resize(T, Vector2::zeros());
1025 apply_control_constraints(&mut controls);
1026
1027 let mut previous_cost = f64::INFINITY;
1028
1029 for _ in 0..QP_MAX_ITERS {
1030 let (x, a_seq, b_seq, _) = linearized_rollout(state, xbar, &controls);
1031 let current_cost = compute_cost(&x, xref, &controls);
1032
1033 let mut gradients = [Vector2::zeros(); T];
1034 let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
1035
1036 for t in (0..T).rev() {
1037 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1038
1039 if t > 0 {
1040 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1041 }
1042 if t < T - 1 {
1043 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1044 }
1045
1046 gradients[t] = grad;
1047
1048 let lx = if t == 0 {
1049 Vector4::zeros()
1050 } else {
1051 (q * state_error(x[t], xref[t])) * 2.0
1052 };
1053 lambda = lx + a_seq[t].transpose() * lambda;
1054 }
1055
1056 let gradient_norm = gradients
1057 .iter()
1058 .map(Vector2::norm_squared)
1059 .sum::<f64>()
1060 .sqrt();
1061
1062 if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1063 break;
1064 }
1065
1066 previous_cost = current_cost;
1067 let current_controls = controls.clone();
1068 let mut step = LINE_SEARCH_INITIAL_STEP;
1069 let mut improved = false;
1070
1071 for _ in 0..LINE_SEARCH_ITERS {
1072 let mut candidate = current_controls.clone();
1073 for t in 0..T {
1074 candidate[t] -= gradients[t] * step;
1075 }
1076 apply_control_constraints(&mut candidate);
1077
1078 let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
1079 let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
1080 if candidate_cost < current_cost {
1081 controls = candidate;
1082 improved = true;
1083 break;
1084 }
1085
1086 step *= 0.5;
1087 }
1088
1089 if !improved {
1090 break;
1091 }
1092 }
1093
1094 controls
1095 }
1096
1097 #[derive(Debug)]
1098 struct SwitchBackTraceRow {
1099 step: usize,
1100 target_index: usize,
1101 state: Vector4<f64>,
1102 xref_head: Vector4<f64>,
1103 xref_tail: Vector4<f64>,
1104 control: Vector2<f64>,
1105 predicted_tail: Vector4<f64>,
1106 }
1107
1108 #[derive(Debug)]
1109 struct SwitchBackDetailedRow {
1110 step: usize,
1111 target_index: usize,
1112 state: Vector4<f64>,
1113 xref: Vec<Vector4<f64>>,
1114 controls: Vec<Vector2<f64>>,
1115 predicted: Vec<Vector4<f64>>,
1116 }
1117
1118 #[derive(Debug)]
1119 struct SwitchBackPlanningSnapshot {
1120 step: usize,
1121 target_index: usize,
1122 state: Vector4<f64>,
1123 xref_head: Vector4<f64>,
1124 warm_start_head: Vector2<f64>,
1125 }
1126
1127 #[derive(Debug)]
1128 struct SwitchBackClosedLoopCheckpoint {
1129 step: usize,
1130 target_index: usize,
1131 goal_distance: f64,
1132 state: Vector4<f64>,
1133 }
1134
1135 #[derive(Debug)]
1136 struct SwitchBackGoalConditionSnapshot {
1137 step: usize,
1138 target_index: usize,
1139 goal_distance: f64,
1140 speed: f64,
1141 speed_abs: f64,
1142 heading_error: f64,
1143 motion_curvature: f64,
1144 near_goal: bool,
1145 near_end: bool,
1146 stopped: bool,
1147 reached_goal: bool,
1148 }
1149
1150 #[derive(Debug)]
1151 struct SwitchBackGoalControlSnapshot {
1152 step: usize,
1153 target_index: usize,
1154 goal_distance: f64,
1155 speed: f64,
1156 accel: f64,
1157 steer: f64,
1158 heading_error: f64,
1159 motion_curvature: f64,
1160 course_curvature: f64,
1161 near_goal: bool,
1162 near_end: bool,
1163 stopped: bool,
1164 reached_goal: bool,
1165 }
1166
1167 type SwitchBackControlTrace = BTreeMap<usize, Vec<Vector2<f64>>>;
1168 type SwitchBackStateTrace = BTreeMap<usize, Vec<Vector4<f64>>>;
1169 type SwitchBackHorizonPair = (Vec<Vector4<f64>>, Vec<Vector4<f64>>);
1170 type SwitchBackHorizonTrace = BTreeMap<usize, SwitchBackHorizonPair>;
1171 type SwitchBackWarmStartTrace = BTreeMap<usize, Vec<Vector2<f64>>>;
1172
1173 #[derive(Debug, Default)]
1174 struct StateInjectionMetrics {
1175 default_same_control0: f64,
1176 default_opposite_control0: f64,
1177 default_same_pred_y: f64,
1178 default_opposite_pred_y: f64,
1179 default_same_pred_v: f64,
1180 default_opposite_pred_v: f64,
1181 expanded_same_control0: f64,
1182 expanded_opposite_control0: f64,
1183 expanded_same_pred_y: f64,
1184 expanded_opposite_pred_y: f64,
1185 expanded_same_pred_v: f64,
1186 expanded_opposite_pred_v: f64,
1187 }
1188
1189 struct SwitchBackContext {
1190 cx: Vec<f64>,
1191 cy: Vec<f64>,
1192 cyaw: Vec<f64>,
1193 speed_profile: Vec<f64>,
1194 state: State,
1195 target_index: usize,
1196 warm_start: Vec<Vector2<f64>>,
1197 }
1198
1199 #[derive(Debug)]
1200 struct SwitchBackTraceDiffSummary {
1201 max_target_index_gap: usize,
1202 max_state_diff: [f64; 4],
1203 max_xref_head_diff: [f64; 4],
1204 max_xref_tail_diff: [f64; 4],
1205 max_control_diff: [f64; 2],
1206 max_predicted_tail_diff: [f64; 4],
1207 }
1208
1209 #[derive(Debug)]
1210 struct SwitchBackDetailedDiffSummary {
1211 max_target_index_gap: usize,
1212 max_state_diff: [f64; 4],
1213 max_xref_diff: [f64; 4],
1214 max_control_diff: [f64; 2],
1215 max_predicted_diff: [f64; 4],
1216 }
1217
1218 fn assert_vec2_close(actual: &Vector2<f64>, expected: [f64; 2], tol: f64) {
1219 for (i, expected_value) in expected.iter().enumerate() {
1220 assert!(
1221 (actual[i] - expected_value).abs() <= tol,
1222 "index {}: actual={} expected={} tol={}",
1223 i,
1224 actual[i],
1225 expected_value,
1226 tol
1227 );
1228 }
1229 }
1230
1231 fn assert_vec4_close(actual: &Vector4<f64>, expected: [f64; 4], tol: f64) {
1232 for (i, expected_value) in expected.iter().enumerate() {
1233 assert!(
1234 (actual[i] - expected_value).abs() <= tol,
1235 "index {}: actual={} expected={} tol={}",
1236 i,
1237 actual[i],
1238 expected_value,
1239 tol
1240 );
1241 }
1242 }
1243
1244 fn parse_switch_back_trace(csv: &str) -> Vec<SwitchBackTraceRow> {
1245 csv.lines()
1246 .skip(1)
1247 .filter(|line| !line.trim().is_empty())
1248 .map(|line| {
1249 let values: Vec<&str> = line.split(',').collect();
1250 assert_eq!(values.len(), 20);
1251
1252 SwitchBackTraceRow {
1253 step: values[0].parse().unwrap(),
1254 target_index: values[1].parse().unwrap(),
1255 state: Vector4::new(
1256 values[2].parse().unwrap(),
1257 values[3].parse().unwrap(),
1258 values[4].parse().unwrap(),
1259 values[5].parse().unwrap(),
1260 ),
1261 xref_head: Vector4::new(
1262 values[6].parse().unwrap(),
1263 values[7].parse().unwrap(),
1264 values[8].parse().unwrap(),
1265 values[9].parse().unwrap(),
1266 ),
1267 xref_tail: Vector4::new(
1268 values[10].parse().unwrap(),
1269 values[11].parse().unwrap(),
1270 values[12].parse().unwrap(),
1271 values[13].parse().unwrap(),
1272 ),
1273 control: Vector2::new(values[14].parse().unwrap(), values[15].parse().unwrap()),
1274 predicted_tail: Vector4::new(
1275 values[16].parse().unwrap(),
1276 values[17].parse().unwrap(),
1277 values[18].parse().unwrap(),
1278 values[19].parse().unwrap(),
1279 ),
1280 }
1281 })
1282 .collect()
1283 }
1284
1285 fn parse_switch_back_reverse_controls(csv: &str) -> SwitchBackControlTrace {
1286 let mut result: SwitchBackControlTrace = BTreeMap::new();
1287
1288 for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
1289 let values: Vec<&str> = line.split(',').collect();
1290 assert_eq!(values.len(), 4);
1291 let step: usize = values[0].parse().unwrap();
1292 let control_index: usize = values[1].parse().unwrap();
1293 let control = Vector2::new(values[2].parse().unwrap(), values[3].parse().unwrap());
1294
1295 let controls = result.entry(step).or_default();
1296 assert_eq!(controls.len(), control_index);
1297 controls.push(control);
1298 }
1299
1300 result
1301 }
1302
1303 fn parse_switch_back_reverse_states(csv: &str) -> SwitchBackStateTrace {
1304 let mut result: SwitchBackStateTrace = BTreeMap::new();
1305
1306 for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
1307 let values: Vec<&str> = line.split(',').collect();
1308 assert_eq!(values.len(), 6);
1309 let step: usize = values[0].parse().unwrap();
1310 let horizon_index: usize = values[1].parse().unwrap();
1311 let state = Vector4::new(
1312 values[2].parse().unwrap(),
1313 values[3].parse().unwrap(),
1314 values[4].parse().unwrap(),
1315 values[5].parse().unwrap(),
1316 );
1317
1318 let states = result.entry(step).or_default();
1319 assert_eq!(states.len(), horizon_index);
1320 states.push(state);
1321 }
1322
1323 result
1324 }
1325
1326 fn parse_switch_back_reverse_horizon(csv: &str) -> SwitchBackHorizonTrace {
1327 let mut result: SwitchBackHorizonTrace = BTreeMap::new();
1328
1329 for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
1330 let values: Vec<&str> = line.split(',').collect();
1331 assert_eq!(values.len(), 10);
1332 let step: usize = values[0].parse().unwrap();
1333 let horizon_index: usize = values[1].parse().unwrap();
1334
1335 let entry = result.entry(step).or_default();
1336 assert_eq!(entry.0.len(), horizon_index);
1337 assert_eq!(entry.1.len(), horizon_index);
1338
1339 entry.0.push(Vector4::new(
1340 values[2].parse().unwrap(),
1341 values[3].parse().unwrap(),
1342 values[4].parse().unwrap(),
1343 values[5].parse().unwrap(),
1344 ));
1345 entry.1.push(Vector4::new(
1346 values[6].parse().unwrap(),
1347 values[7].parse().unwrap(),
1348 values[8].parse().unwrap(),
1349 values[9].parse().unwrap(),
1350 ));
1351 }
1352
1353 result
1354 }
1355
1356 fn trace_state_diff(actual: &Vector4<f64>, expected: &Vector4<f64>) -> [f64; 4] {
1357 [
1358 (actual[0] - expected[0]).abs(),
1359 (actual[1] - expected[1]).abs(),
1360 (actual[2] - expected[2]).abs(),
1361 angle_diff(actual[3], expected[3]).abs(),
1362 ]
1363 }
1364
1365 fn trace_control_diff(actual: &Vector2<f64>, expected: &Vector2<f64>) -> [f64; 2] {
1366 [
1367 (actual[0] - expected[0]).abs(),
1368 (actual[1] - expected[1]).abs(),
1369 ]
1370 }
1371
1372 fn summarize_switch_back_trace_window(
1373 actual: &[SwitchBackTraceRow],
1374 expected: &[SwitchBackTraceRow],
1375 ) -> SwitchBackTraceDiffSummary {
1376 let mut summary = SwitchBackTraceDiffSummary {
1377 max_target_index_gap: 0,
1378 max_state_diff: [0.0; 4],
1379 max_xref_head_diff: [0.0; 4],
1380 max_xref_tail_diff: [0.0; 4],
1381 max_control_diff: [0.0; 2],
1382 max_predicted_tail_diff: [0.0; 4],
1383 };
1384
1385 assert_eq!(actual.len(), expected.len());
1386
1387 for (actual_row, expected_row) in actual.iter().zip(expected.iter()) {
1388 assert_eq!(actual_row.step, expected_row.step);
1389 summary.max_target_index_gap = summary
1390 .max_target_index_gap
1391 .max(actual_row.target_index.abs_diff(expected_row.target_index));
1392
1393 for (max_diff, diff) in summary
1394 .max_state_diff
1395 .iter_mut()
1396 .zip(trace_state_diff(&actual_row.state, &expected_row.state))
1397 {
1398 *max_diff = (*max_diff).max(diff);
1399 }
1400
1401 for (max_diff, diff) in summary.max_xref_head_diff.iter_mut().zip(trace_state_diff(
1402 &actual_row.xref_head,
1403 &expected_row.xref_head,
1404 )) {
1405 *max_diff = (*max_diff).max(diff);
1406 }
1407
1408 for (max_diff, diff) in summary.max_xref_tail_diff.iter_mut().zip(trace_state_diff(
1409 &actual_row.xref_tail,
1410 &expected_row.xref_tail,
1411 )) {
1412 *max_diff = (*max_diff).max(diff);
1413 }
1414
1415 for (max_diff, diff) in summary.max_control_diff.iter_mut().zip(trace_control_diff(
1416 &actual_row.control,
1417 &expected_row.control,
1418 )) {
1419 *max_diff = (*max_diff).max(diff);
1420 }
1421
1422 for (max_diff, diff) in
1423 summary
1424 .max_predicted_tail_diff
1425 .iter_mut()
1426 .zip(trace_state_diff(
1427 &actual_row.predicted_tail,
1428 &expected_row.predicted_tail,
1429 ))
1430 {
1431 *max_diff = (*max_diff).max(diff);
1432 }
1433 }
1434
1435 summary
1436 }
1437
1438 fn summarize_switch_back_detailed_window(
1439 actual_trace: &[SwitchBackDetailedRow],
1440 expected_trace: &[SwitchBackTraceRow],
1441 expected_controls: &SwitchBackControlTrace,
1442 expected_horizon: &SwitchBackHorizonTrace,
1443 step_window: std::ops::RangeInclusive<usize>,
1444 ) -> SwitchBackDetailedDiffSummary {
1445 let mut summary = SwitchBackDetailedDiffSummary {
1446 max_target_index_gap: 0,
1447 max_state_diff: [0.0; 4],
1448 max_xref_diff: [0.0; 4],
1449 max_control_diff: [0.0; 2],
1450 max_predicted_diff: [0.0; 4],
1451 };
1452
1453 for actual_row in actual_trace
1454 .iter()
1455 .filter(|row| step_window.contains(&row.step))
1456 {
1457 let expected_row = &expected_trace[actual_row.step];
1458 assert_eq!(actual_row.step, expected_row.step);
1459 summary.max_target_index_gap = summary
1460 .max_target_index_gap
1461 .max(actual_row.target_index.abs_diff(expected_row.target_index));
1462
1463 for (slot, diff) in summary
1464 .max_state_diff
1465 .iter_mut()
1466 .zip(trace_state_diff(&actual_row.state, &expected_row.state))
1467 {
1468 *slot = (*slot).max(diff);
1469 }
1470
1471 let expected_controls_for_step = expected_controls.get(&actual_row.step).unwrap();
1472 let (expected_xref_for_step, expected_predicted_for_step) =
1473 expected_horizon.get(&actual_row.step).unwrap();
1474 let expected_xref_rows = expected_xref_for_step
1475 .iter()
1476 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
1477 .collect::<Vec<_>>();
1478 let expected_control_rows = expected_controls_for_step
1479 .iter()
1480 .map(|vec| [vec[0], vec[1]])
1481 .collect::<Vec<_>>();
1482 let expected_predicted_rows = expected_predicted_for_step
1483 .iter()
1484 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
1485 .collect::<Vec<_>>();
1486
1487 for (slot, diff) in summary
1488 .max_xref_diff
1489 .iter_mut()
1490 .zip(summarize_vec4_sequence_diff(
1491 &actual_row.xref,
1492 &expected_xref_rows,
1493 ))
1494 {
1495 *slot = (*slot).max(diff);
1496 }
1497
1498 for (slot, diff) in
1499 summary
1500 .max_control_diff
1501 .iter_mut()
1502 .zip(summarize_vec2_sequence_diff(
1503 &actual_row.controls,
1504 &expected_control_rows,
1505 ))
1506 {
1507 *slot = (*slot).max(diff);
1508 }
1509
1510 for (slot, diff) in
1511 summary
1512 .max_predicted_diff
1513 .iter_mut()
1514 .zip(summarize_vec4_sequence_diff(
1515 &actual_row.predicted,
1516 &expected_predicted_rows,
1517 ))
1518 {
1519 *slot = (*slot).max(diff);
1520 }
1521 }
1522
1523 summary
1524 }
1525
1526 fn assert_diff_bounds<const N: usize>(label: &str, actual: [f64; N], tolerance: [f64; N]) {
1527 for (i, (actual_diff, bound)) in actual.into_iter().zip(tolerance).enumerate() {
1528 assert!(
1529 actual_diff <= bound,
1530 "{} diff index {}: actual={} tolerance={}",
1531 label,
1532 i,
1533 actual_diff,
1534 bound
1535 );
1536 }
1537 }
1538
1539 fn summarize_vec4_sequence_diff(actual: &[Vector4<f64>], expected: &[[f64; 4]]) -> [f64; 4] {
1540 assert_eq!(actual.len(), expected.len());
1541 let mut max_diff: [f64; 4] = [0.0; 4];
1542
1543 for (actual_vec, expected_vec) in actual.iter().zip(expected.iter()) {
1544 for (slot, diff) in max_diff.iter_mut().zip(trace_state_diff(
1545 actual_vec,
1546 &Vector4::from_row_slice(expected_vec),
1547 )) {
1548 *slot = (*slot).max(diff);
1549 }
1550 }
1551
1552 max_diff
1553 }
1554
1555 fn summarize_vec2_sequence_diff(actual: &[Vector2<f64>], expected: &[[f64; 2]]) -> [f64; 2] {
1556 assert_eq!(actual.len(), expected.len());
1557 let mut max_diff: [f64; 2] = [0.0; 2];
1558
1559 for (actual_vec, expected_vec) in actual.iter().zip(expected.iter()) {
1560 for (slot, diff) in max_diff.iter_mut().zip(trace_control_diff(
1561 actual_vec,
1562 &Vector2::from_row_slice(expected_vec),
1563 )) {
1564 *slot = (*slot).max(diff);
1565 }
1566 }
1567
1568 max_diff
1569 }
1570
1571 fn compute_cost_without_angle_wrap(
1572 x: &[Vector4<f64>],
1573 xref: &[Vector4<f64>],
1574 controls: &[Vector2<f64>],
1575 ) -> f64 {
1576 let q = state_cost_weight();
1577 let qf = terminal_cost_weight();
1578 let r = control_cost_weight();
1579 let rd = control_rate_weight();
1580
1581 let mut cost = 0.0;
1582 for t in 0..T {
1583 cost += controls[t].dot(&(r * controls[t]));
1584 if t != 0 {
1585 let err = x[t] - xref[t];
1586 cost += err.dot(&(q * err));
1587 }
1588 if t < T - 1 {
1589 let du = controls[t + 1] - controls[t];
1590 cost += du.dot(&(rd * du));
1591 }
1592 }
1593
1594 let terminal_error = x[T] - xref[T];
1595 cost + terminal_error.dot(&(qf * terminal_error))
1596 }
1597
1598 #[derive(Debug)]
1599 struct GradientTraceEntry {
1600 iter: usize,
1601 cost: f64,
1602 gradient_norm: f64,
1603 accepted_step: Option<f64>,
1604 }
1605
1606 #[derive(Debug)]
1607 struct LineSearchProjectionEntry {
1608 step: f64,
1609 raw_cost: f64,
1610 projected_cost: f64,
1611 raw_to_projected: [f64; 2],
1612 raw_to_python: [f64; 2],
1613 projected_to_python: [f64; 2],
1614 }
1615
1616 #[derive(Debug)]
1617 struct ProjectionTraceEntry {
1618 iter: usize,
1619 current_cost: f64,
1620 candidates: Vec<LineSearchProjectionEntry>,
1621 }
1622
1623 fn control_sequence_max_diff(actual: &[Vector2<f64>], expected: &[Vector2<f64>]) -> [f64; 2] {
1624 assert_eq!(actual.len(), expected.len());
1625 let mut max_diff: [f64; 2] = [0.0; 2];
1626
1627 for (actual_vec, expected_vec) in actual.iter().zip(expected.iter()) {
1628 let diff = trace_control_diff(actual_vec, expected_vec);
1629 max_diff[0] = max_diff[0].max(diff[0]);
1630 max_diff[1] = max_diff[1].max(diff[1]);
1631 }
1632
1633 max_diff
1634 }
1635
1636 fn linearized_rollout_without_angle_wrap(
1637 x0: &State,
1638 xbar: &[Vector4<f64>],
1639 controls: &[Vector2<f64>],
1640 ) -> Vec<Vector4<f64>> {
1641 let mut x = vec![Vector4::zeros(); T + 1];
1642 x[0] = x0.to_vector();
1643
1644 for t in 0..T {
1645 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
1646 x[t + 1] = a * x[t] + b * controls[t] + c;
1647 }
1648
1649 x
1650 }
1651
1652 fn optimize_linearized_controls_without_angle_wrap_with_trace(
1653 xref: &[Vector4<f64>],
1654 xbar: &[Vector4<f64>],
1655 state: &State,
1656 initial_controls: &[Vector2<f64>],
1657 max_iters: usize,
1658 line_search_iters: usize,
1659 ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1660 optimize_linearized_controls_without_angle_wrap_with_trace_and_initial_step(
1661 xref,
1662 xbar,
1663 state,
1664 initial_controls,
1665 max_iters,
1666 line_search_iters,
1667 0.25,
1668 )
1669 }
1670
1671 fn optimize_linearized_controls_without_angle_wrap_with_trace_and_initial_step(
1672 xref: &[Vector4<f64>],
1673 xbar: &[Vector4<f64>],
1674 state: &State,
1675 initial_controls: &[Vector2<f64>],
1676 max_iters: usize,
1677 line_search_iters: usize,
1678 initial_step: f64,
1679 ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1680 let q = state_cost_weight();
1681 let qf = terminal_cost_weight();
1682 let r = control_cost_weight();
1683 let rd = control_rate_weight();
1684
1685 let mut controls = initial_controls.to_vec();
1686 controls.resize(T, Vector2::zeros());
1687 apply_control_constraints(&mut controls);
1688
1689 let mut previous_cost = f64::INFINITY;
1690 let mut trace = Vec::new();
1691
1692 for iter in 0..max_iters {
1693 let mut x = vec![Vector4::zeros(); T + 1];
1694 let mut a_seq = Vec::with_capacity(T);
1695 let mut b_seq = Vec::with_capacity(T);
1696 x[0] = state.to_vector();
1697
1698 for t in 0..T {
1699 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
1700 x[t + 1] = a * x[t] + b * controls[t] + c;
1701 a_seq.push(a);
1702 b_seq.push(b);
1703 }
1704 let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
1705
1706 let mut gradients = [Vector2::zeros(); T];
1707 let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
1708
1709 for t in (0..T).rev() {
1710 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1711
1712 if t > 0 {
1713 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1714 }
1715 if t < T - 1 {
1716 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1717 }
1718
1719 gradients[t] = grad;
1720
1721 let lx = if t == 0 {
1722 Vector4::zeros()
1723 } else {
1724 (q * (x[t] - xref[t])) * 2.0
1725 };
1726 lambda = lx + a_seq[t].transpose() * lambda;
1727 }
1728
1729 let gradient_norm = gradients
1730 .iter()
1731 .map(Vector2::norm_squared)
1732 .sum::<f64>()
1733 .sqrt();
1734
1735 if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1736 trace.push(GradientTraceEntry {
1737 iter,
1738 cost: current_cost,
1739 gradient_norm,
1740 accepted_step: None,
1741 });
1742 break;
1743 }
1744
1745 previous_cost = current_cost;
1746 let current_controls = controls.clone();
1747 let mut step = initial_step;
1748 let mut accepted_step = None;
1749
1750 for _ in 0..line_search_iters {
1751 let mut candidate = current_controls.clone();
1752 for t in 0..T {
1753 candidate[t] -= gradients[t] * step;
1754 }
1755 apply_control_constraints(&mut candidate);
1756
1757 let candidate_x = linearized_rollout_without_angle_wrap(state, xbar, &candidate);
1758 let candidate_cost =
1759 compute_cost_without_angle_wrap(&candidate_x, xref, &candidate);
1760 if candidate_cost < current_cost {
1761 controls = candidate;
1762 accepted_step = Some(step);
1763 break;
1764 }
1765
1766 step *= 0.5;
1767 }
1768
1769 trace.push(GradientTraceEntry {
1770 iter,
1771 cost: current_cost,
1772 gradient_norm,
1773 accepted_step,
1774 });
1775
1776 if accepted_step.is_none() {
1777 break;
1778 }
1779 }
1780
1781 (controls, trace)
1782 }
1783
1784 fn optimize_linearized_controls_without_angle_wrap_with_candidate_expansion(
1785 xref: &[Vector4<f64>],
1786 xbar: &[Vector4<f64>],
1787 state: &State,
1788 initial_controls: &[Vector2<f64>],
1789 max_iters: usize,
1790 step_candidates: &[f64],
1791 ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1792 let q = state_cost_weight();
1793 let qf = terminal_cost_weight();
1794 let r = control_cost_weight();
1795 let rd = control_rate_weight();
1796
1797 let mut controls = initial_controls.to_vec();
1798 controls.resize(T, Vector2::zeros());
1799 apply_control_constraints(&mut controls);
1800
1801 let mut previous_cost = f64::INFINITY;
1802 let mut trace = Vec::new();
1803
1804 for iter in 0..max_iters {
1805 let mut x = vec![Vector4::zeros(); T + 1];
1806 let mut a_seq = Vec::with_capacity(T);
1807 let mut b_seq = Vec::with_capacity(T);
1808 x[0] = state.to_vector();
1809
1810 for t in 0..T {
1811 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
1812 x[t + 1] = a * x[t] + b * controls[t] + c;
1813 a_seq.push(a);
1814 b_seq.push(b);
1815 }
1816 let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
1817
1818 let mut gradients = [Vector2::zeros(); T];
1819 let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
1820
1821 for t in (0..T).rev() {
1822 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1823
1824 if t > 0 {
1825 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1826 }
1827 if t < T - 1 {
1828 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1829 }
1830
1831 gradients[t] = grad;
1832
1833 let lx = if t == 0 {
1834 Vector4::zeros()
1835 } else {
1836 (q * (x[t] - xref[t])) * 2.0
1837 };
1838 lambda = lx + a_seq[t].transpose() * lambda;
1839 }
1840
1841 let gradient_norm = gradients
1842 .iter()
1843 .map(Vector2::norm_squared)
1844 .sum::<f64>()
1845 .sqrt();
1846
1847 if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1848 trace.push(GradientTraceEntry {
1849 iter,
1850 cost: current_cost,
1851 gradient_norm,
1852 accepted_step: None,
1853 });
1854 break;
1855 }
1856
1857 previous_cost = current_cost;
1858 let current_controls = controls.clone();
1859 let mut accepted_step = None;
1860 let mut best_candidate_controls = None;
1861 let mut best_candidate_cost = current_cost;
1862
1863 for &step in step_candidates {
1864 let mut candidate = current_controls.clone();
1865 for t in 0..T {
1866 candidate[t] -= gradients[t] * step;
1867 }
1868 apply_control_constraints(&mut candidate);
1869
1870 let candidate_x = linearized_rollout_without_angle_wrap(state, xbar, &candidate);
1871 let candidate_cost =
1872 compute_cost_without_angle_wrap(&candidate_x, xref, &candidate);
1873 if candidate_cost < best_candidate_cost {
1874 best_candidate_cost = candidate_cost;
1875 accepted_step = Some(step);
1876 best_candidate_controls = Some(candidate);
1877 }
1878 }
1879
1880 trace.push(GradientTraceEntry {
1881 iter,
1882 cost: current_cost,
1883 gradient_norm,
1884 accepted_step,
1885 });
1886
1887 if let Some(candidate) = best_candidate_controls {
1888 controls = candidate;
1889 } else {
1890 break;
1891 }
1892 }
1893
1894 (controls, trace)
1895 }
1896
1897 fn optimize_linearized_controls_with_candidate_expansion(
1898 xref: &[Vector4<f64>],
1899 xbar: &[Vector4<f64>],
1900 state: &State,
1901 initial_controls: &[Vector2<f64>],
1902 max_iters: usize,
1903 step_candidates: &[f64],
1904 ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1905 let q = state_cost_weight();
1906 let qf = terminal_cost_weight();
1907 let r = control_cost_weight();
1908 let rd = control_rate_weight();
1909
1910 let mut controls = initial_controls.to_vec();
1911 controls.resize(T, Vector2::zeros());
1912 apply_control_constraints(&mut controls);
1913
1914 let mut previous_cost = f64::INFINITY;
1915 let mut trace = Vec::new();
1916
1917 for iter in 0..max_iters {
1918 let (x, a_seq, b_seq, _) = linearized_rollout(state, xbar, &controls);
1919 let current_cost = compute_cost(&x, xref, &controls);
1920
1921 let mut gradients = [Vector2::zeros(); T];
1922 let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
1923
1924 for t in (0..T).rev() {
1925 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1926
1927 if t > 0 {
1928 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1929 }
1930 if t < T - 1 {
1931 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1932 }
1933
1934 gradients[t] = grad;
1935
1936 let lx = if t == 0 {
1937 Vector4::zeros()
1938 } else {
1939 (q * state_error(x[t], xref[t])) * 2.0
1940 };
1941 lambda = lx + a_seq[t].transpose() * lambda;
1942 }
1943
1944 let gradient_norm = gradients
1945 .iter()
1946 .map(Vector2::norm_squared)
1947 .sum::<f64>()
1948 .sqrt();
1949
1950 if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1951 trace.push(GradientTraceEntry {
1952 iter,
1953 cost: current_cost,
1954 gradient_norm,
1955 accepted_step: None,
1956 });
1957 break;
1958 }
1959
1960 previous_cost = current_cost;
1961 let current_controls = controls.clone();
1962 let mut accepted_step = None;
1963 let mut best_candidate_controls = None;
1964 let mut best_candidate_cost = current_cost;
1965
1966 for &step in step_candidates {
1967 let mut candidate = current_controls.clone();
1968 for t in 0..T {
1969 candidate[t] -= gradients[t] * step;
1970 }
1971 apply_control_constraints(&mut candidate);
1972
1973 let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
1974 let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
1975 if candidate_cost < best_candidate_cost {
1976 best_candidate_cost = candidate_cost;
1977 accepted_step = Some(step);
1978 best_candidate_controls = Some(candidate);
1979 }
1980 }
1981
1982 trace.push(GradientTraceEntry {
1983 iter,
1984 cost: current_cost,
1985 gradient_norm,
1986 accepted_step,
1987 });
1988
1989 if let Some(candidate) = best_candidate_controls {
1990 controls = candidate;
1991 } else {
1992 break;
1993 }
1994 }
1995
1996 (controls, trace)
1997 }
1998
1999 fn expanded_backtracking_step_candidates() -> Vec<f64> {
2000 let mut steps = vec![1.0, 0.5];
2001 let mut step = LINE_SEARCH_INITIAL_STEP;
2002 for _ in 0..LINE_SEARCH_ITERS {
2003 steps.push(step);
2004 step *= 0.5;
2005 }
2006 steps
2007 }
2008
2009 fn optimize_linearized_controls_with_full_candidate_expansion(
2010 xref: &[Vector4<f64>],
2011 xbar: &[Vector4<f64>],
2012 state: &State,
2013 initial_controls: &[Vector2<f64>],
2014 ) -> Vec<Vector2<f64>> {
2015 optimize_linearized_controls_with_candidate_expansion(
2016 xref,
2017 xbar,
2018 state,
2019 initial_controls,
2020 QP_MAX_ITERS,
2021 &expanded_backtracking_step_candidates(),
2022 )
2023 .0
2024 }
2025
2026 fn iterative_linear_mpc_control_with_test_optimizer<F>(
2027 xref: &[Vector4<f64>],
2028 state: &State,
2029 warm_start: &[Vector2<f64>],
2030 optimize: F,
2031 ) -> MpcResult
2032 where
2033 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>,
2034 {
2035 let mut controls = warm_start.to_vec();
2036 controls.resize(T, Vector2::zeros());
2037 apply_control_constraints(&mut controls);
2038
2039 let mut predicted = predict_motion(*state, &controls);
2040 for _ in 0..MAX_ITER {
2041 let previous_controls = controls.clone();
2042 controls = optimize(xref, &predicted, state, &controls);
2043 predicted = predict_motion(*state, &controls);
2044
2045 let du = controls
2046 .iter()
2047 .zip(previous_controls.iter())
2048 .map(|(current, previous)| {
2049 (current[0] - previous[0]).abs() + (current[1] - previous[1]).abs()
2050 })
2051 .sum::<f64>();
2052
2053 if du <= DU_TH {
2054 break;
2055 }
2056 }
2057
2058 MpcResult {
2059 controls,
2060 predicted,
2061 }
2062 }
2063
2064 fn trace_projection_effect_against_python_reference(
2065 xref: &[Vector4<f64>],
2066 xbar: &[Vector4<f64>],
2067 state: &State,
2068 initial_controls: &[Vector2<f64>],
2069 python_controls: &[Vector2<f64>],
2070 max_iters: usize,
2071 line_search_iters: usize,
2072 ) -> Vec<ProjectionTraceEntry> {
2073 let q = state_cost_weight();
2074 let qf = terminal_cost_weight();
2075 let r = control_cost_weight();
2076 let rd = control_rate_weight();
2077
2078 let mut controls = initial_controls.to_vec();
2079 controls.resize(T, Vector2::zeros());
2080 apply_control_constraints(&mut controls);
2081
2082 let mut previous_cost = f64::INFINITY;
2083 let mut trace = Vec::new();
2084
2085 for iter in 0..max_iters {
2086 let mut x = vec![Vector4::zeros(); T + 1];
2087 let mut a_seq = Vec::with_capacity(T);
2088 let mut b_seq = Vec::with_capacity(T);
2089 x[0] = state.to_vector();
2090
2091 for t in 0..T {
2092 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
2093 x[t + 1] = a * x[t] + b * controls[t] + c;
2094 a_seq.push(a);
2095 b_seq.push(b);
2096 }
2097 let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
2098
2099 let mut gradients = [Vector2::zeros(); T];
2100 let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
2101
2102 for t in (0..T).rev() {
2103 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
2104
2105 if t > 0 {
2106 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
2107 }
2108 if t < T - 1 {
2109 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
2110 }
2111
2112 gradients[t] = grad;
2113
2114 let lx = if t == 0 {
2115 Vector4::zeros()
2116 } else {
2117 (q * (x[t] - xref[t])) * 2.0
2118 };
2119 lambda = lx + a_seq[t].transpose() * lambda;
2120 }
2121
2122 let gradient_norm = gradients
2123 .iter()
2124 .map(Vector2::norm_squared)
2125 .sum::<f64>()
2126 .sqrt();
2127
2128 if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
2129 break;
2130 }
2131
2132 previous_cost = current_cost;
2133 let current_controls = controls.clone();
2134 let mut step = 0.25;
2135 let mut candidates = Vec::new();
2136 let mut improved = false;
2137
2138 for _ in 0..line_search_iters {
2139 let mut raw_candidate = current_controls.clone();
2140 for t in 0..T {
2141 raw_candidate[t] -= gradients[t] * step;
2142 }
2143
2144 let raw_x = linearized_rollout_without_angle_wrap(state, xbar, &raw_candidate);
2145 let raw_cost = compute_cost_without_angle_wrap(&raw_x, xref, &raw_candidate);
2146
2147 let mut projected_candidate = raw_candidate.clone();
2148 apply_control_constraints(&mut projected_candidate);
2149 let projected_x =
2150 linearized_rollout_without_angle_wrap(state, xbar, &projected_candidate);
2151 let projected_cost =
2152 compute_cost_without_angle_wrap(&projected_x, xref, &projected_candidate);
2153
2154 candidates.push(LineSearchProjectionEntry {
2155 step,
2156 raw_cost,
2157 projected_cost,
2158 raw_to_projected: control_sequence_max_diff(
2159 &raw_candidate,
2160 &projected_candidate,
2161 ),
2162 raw_to_python: control_sequence_max_diff(&raw_candidate, python_controls),
2163 projected_to_python: control_sequence_max_diff(
2164 &projected_candidate,
2165 python_controls,
2166 ),
2167 });
2168
2169 if projected_cost < current_cost {
2170 controls = projected_candidate;
2171 improved = true;
2172 break;
2173 }
2174
2175 step *= 0.5;
2176 }
2177
2178 trace.push(ProjectionTraceEntry {
2179 iter,
2180 current_cost,
2181 candidates,
2182 });
2183
2184 if !improved {
2185 break;
2186 }
2187 }
2188
2189 trace
2190 }
2191
2192 fn first_iteration_projection_candidates(
2193 xref: &[Vector4<f64>],
2194 xbar: &[Vector4<f64>],
2195 state: &State,
2196 initial_controls: &[Vector2<f64>],
2197 python_controls: &[Vector2<f64>],
2198 step_sizes: &[f64],
2199 ) -> (f64, Vec<LineSearchProjectionEntry>) {
2200 let q = state_cost_weight();
2201 let qf = terminal_cost_weight();
2202 let r = control_cost_weight();
2203 let rd = control_rate_weight();
2204
2205 let mut controls = initial_controls.to_vec();
2206 controls.resize(T, Vector2::zeros());
2207 apply_control_constraints(&mut controls);
2208
2209 let mut x = vec![Vector4::zeros(); T + 1];
2210 let mut a_seq = Vec::with_capacity(T);
2211 let mut b_seq = Vec::with_capacity(T);
2212 x[0] = state.to_vector();
2213
2214 for t in 0..T {
2215 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
2216 x[t + 1] = a * x[t] + b * controls[t] + c;
2217 a_seq.push(a);
2218 b_seq.push(b);
2219 }
2220
2221 let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
2222 let mut gradients = [Vector2::zeros(); T];
2223 let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
2224
2225 for t in (0..T).rev() {
2226 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
2227
2228 if t > 0 {
2229 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
2230 }
2231 if t < T - 1 {
2232 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
2233 }
2234
2235 gradients[t] = grad;
2236
2237 let lx = if t == 0 {
2238 Vector4::zeros()
2239 } else {
2240 (q * (x[t] - xref[t])) * 2.0
2241 };
2242 lambda = lx + a_seq[t].transpose() * lambda;
2243 }
2244
2245 let mut candidates = Vec::new();
2246 for &step in step_sizes {
2247 let mut raw_candidate = controls.clone();
2248 for t in 0..T {
2249 raw_candidate[t] -= gradients[t] * step;
2250 }
2251
2252 let raw_x = linearized_rollout_without_angle_wrap(state, xbar, &raw_candidate);
2253 let raw_cost = compute_cost_without_angle_wrap(&raw_x, xref, &raw_candidate);
2254
2255 let mut projected_candidate = raw_candidate.clone();
2256 apply_control_constraints(&mut projected_candidate);
2257 let projected_x =
2258 linearized_rollout_without_angle_wrap(state, xbar, &projected_candidate);
2259 let projected_cost =
2260 compute_cost_without_angle_wrap(&projected_x, xref, &projected_candidate);
2261
2262 candidates.push(LineSearchProjectionEntry {
2263 step,
2264 raw_cost,
2265 projected_cost,
2266 raw_to_projected: control_sequence_max_diff(&raw_candidate, &projected_candidate),
2267 raw_to_python: control_sequence_max_diff(&raw_candidate, python_controls),
2268 projected_to_python: control_sequence_max_diff(
2269 &projected_candidate,
2270 python_controls,
2271 ),
2272 });
2273 }
2274
2275 (current_cost, candidates)
2276 }
2277
2278 fn state_from_vector4(state: &Vector4<f64>) -> State {
2279 State::new(state[0], state[1], state[2], state[3])
2280 }
2281
2282 fn inject_state_components(
2283 base: &Vector4<f64>,
2284 injected: &Vector4<f64>,
2285 component_indices: &[usize],
2286 ) -> State {
2287 let mut hybrid = *base;
2288 for index in component_indices {
2289 hybrid[*index] = injected[*index];
2290 }
2291 state_from_vector4(&hybrid)
2292 }
2293
2294 fn measure_switch_back_state_injection(indices: &[usize]) -> StateInjectionMetrics {
2295 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2296 330,
2297 optimize_linearized_controls_original,
2298 );
2299 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2300 330,
2301 optimize_linearized_controls_with_full_candidate_expansion,
2302 );
2303 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2304 330,
2305 optimize_linearized_controls_original,
2306 );
2307 let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2308 330,
2309 optimize_linearized_controls_with_full_candidate_expansion,
2310 );
2311
2312 let mut summary = StateInjectionMetrics::default();
2313 for metrics in measure_switch_back_state_injection_by_step_with_context(
2314 indices,
2315 &[325usize, 326, 327, 328, 329],
2316 &default_trace,
2317 &expanded_trace,
2318 &default_warm_start,
2319 &expanded_warm_start,
2320 )
2321 .into_values()
2322 {
2323 summary.default_same_control0 += metrics.default_same_control0;
2324 summary.default_opposite_control0 += metrics.default_opposite_control0;
2325 summary.default_same_pred_y += metrics.default_same_pred_y;
2326 summary.default_opposite_pred_y += metrics.default_opposite_pred_y;
2327 summary.default_same_pred_v += metrics.default_same_pred_v;
2328 summary.default_opposite_pred_v += metrics.default_opposite_pred_v;
2329 summary.expanded_same_control0 += metrics.expanded_same_control0;
2330 summary.expanded_opposite_control0 += metrics.expanded_opposite_control0;
2331 summary.expanded_same_pred_y += metrics.expanded_same_pred_y;
2332 summary.expanded_opposite_pred_y += metrics.expanded_opposite_pred_y;
2333 summary.expanded_same_pred_v += metrics.expanded_same_pred_v;
2334 summary.expanded_opposite_pred_v += metrics.expanded_opposite_pred_v;
2335 }
2336
2337 summary
2338 }
2339
2340 fn measure_switch_back_state_injection_by_step_with_context(
2341 indices: &[usize],
2342 steps: &[usize],
2343 default_trace: &[SwitchBackDetailedRow],
2344 expanded_trace: &[SwitchBackDetailedRow],
2345 default_warm_start: &SwitchBackWarmStartTrace,
2346 expanded_warm_start: &SwitchBackWarmStartTrace,
2347 ) -> BTreeMap<usize, StateInjectionMetrics> {
2348 let mut by_step = BTreeMap::new();
2349
2350 for step in steps {
2351 let mut metrics = StateInjectionMetrics::default();
2352 let default_row = detailed_row_at_step(default_trace, *step);
2353 let expanded_row = detailed_row_at_step(expanded_trace, *step);
2354 let default_injected = iterative_linear_mpc_control_with_test_optimizer(
2355 &default_row.xref,
2356 &inject_state_components(&default_row.state, &expanded_row.state, indices),
2357 default_warm_start.get(step).unwrap(),
2358 optimize_linearized_controls_original,
2359 );
2360 let expanded_injected = iterative_linear_mpc_control_with_test_optimizer(
2361 &expanded_row.xref,
2362 &inject_state_components(&expanded_row.state, &default_row.state, indices),
2363 expanded_warm_start.get(step).unwrap(),
2364 optimize_linearized_controls_with_full_candidate_expansion,
2365 );
2366
2367 let default_baseline_controls = default_row
2368 .controls
2369 .iter()
2370 .map(|vec| [vec[0], vec[1]])
2371 .collect::<Vec<_>>();
2372 let expanded_baseline_controls = expanded_row
2373 .controls
2374 .iter()
2375 .map(|vec| [vec[0], vec[1]])
2376 .collect::<Vec<_>>();
2377 let default_baseline_predicted = default_row
2378 .predicted
2379 .iter()
2380 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
2381 .collect::<Vec<_>>();
2382 let expanded_baseline_predicted = expanded_row
2383 .predicted
2384 .iter()
2385 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
2386 .collect::<Vec<_>>();
2387
2388 let default_to_default_control = summarize_vec2_sequence_diff(
2389 &default_injected.controls,
2390 &default_baseline_controls,
2391 );
2392 let default_to_expanded_control = summarize_vec2_sequence_diff(
2393 &default_injected.controls,
2394 &expanded_baseline_controls,
2395 );
2396 let default_to_default_predicted = summarize_vec4_sequence_diff(
2397 &default_injected.predicted,
2398 &default_baseline_predicted,
2399 );
2400 let default_to_expanded_predicted = summarize_vec4_sequence_diff(
2401 &default_injected.predicted,
2402 &expanded_baseline_predicted,
2403 );
2404
2405 let expanded_to_expanded_control = summarize_vec2_sequence_diff(
2406 &expanded_injected.controls,
2407 &expanded_baseline_controls,
2408 );
2409 let expanded_to_default_control = summarize_vec2_sequence_diff(
2410 &expanded_injected.controls,
2411 &default_baseline_controls,
2412 );
2413 let expanded_to_expanded_predicted = summarize_vec4_sequence_diff(
2414 &expanded_injected.predicted,
2415 &expanded_baseline_predicted,
2416 );
2417 let expanded_to_default_predicted = summarize_vec4_sequence_diff(
2418 &expanded_injected.predicted,
2419 &default_baseline_predicted,
2420 );
2421
2422 metrics.default_same_control0 = default_to_default_control[0];
2423 metrics.default_opposite_control0 = default_to_expanded_control[0];
2424 metrics.default_same_pred_y = default_to_default_predicted[1];
2425 metrics.default_opposite_pred_y = default_to_expanded_predicted[1];
2426 metrics.default_same_pred_v = default_to_default_predicted[2];
2427 metrics.default_opposite_pred_v = default_to_expanded_predicted[2];
2428
2429 metrics.expanded_same_control0 = expanded_to_expanded_control[0];
2430 metrics.expanded_opposite_control0 = expanded_to_default_control[0];
2431 metrics.expanded_same_pred_y = expanded_to_expanded_predicted[1];
2432 metrics.expanded_opposite_pred_y = expanded_to_default_predicted[1];
2433 metrics.expanded_same_pred_v = expanded_to_expanded_predicted[2];
2434 metrics.expanded_opposite_pred_v = expanded_to_default_predicted[2];
2435
2436 by_step.insert(*step, metrics);
2437 }
2438
2439 by_step
2440 }
2441
2442 fn measure_switch_back_state_injection_by_step(
2443 indices: &[usize],
2444 steps: &[usize],
2445 ) -> BTreeMap<usize, StateInjectionMetrics> {
2446 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2447 330,
2448 optimize_linearized_controls_original,
2449 );
2450 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2451 330,
2452 optimize_linearized_controls_with_full_candidate_expansion,
2453 );
2454 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2455 330,
2456 optimize_linearized_controls_original,
2457 );
2458 let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2459 330,
2460 optimize_linearized_controls_with_full_candidate_expansion,
2461 );
2462
2463 measure_switch_back_state_injection_by_step_with_context(
2464 indices,
2465 steps,
2466 &default_trace,
2467 &expanded_trace,
2468 &default_warm_start,
2469 &expanded_warm_start,
2470 )
2471 }
2472
2473 fn state_injection_metrics_at_step(
2474 metrics: &BTreeMap<usize, StateInjectionMetrics>,
2475 step: usize,
2476 ) -> &StateInjectionMetrics {
2477 metrics
2478 .get(&step)
2479 .unwrap_or_else(|| panic!("missing state-injection metrics for step {step}"))
2480 }
2481
2482 fn advance_switch_back_context(steps: usize) -> SwitchBackContext {
2483 let (cx, cy, cyaw) = generate_switch_back_course();
2484 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2485
2486 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2487 if state.yaw - cyaw[0] >= PI {
2488 state.yaw -= 2.0 * PI;
2489 } else if state.yaw - cyaw[0] <= -PI {
2490 state.yaw += 2.0 * PI;
2491 }
2492
2493 let mut target_index = 0;
2494 let mut warm_start = vec![Vector2::zeros(); T];
2495
2496 for _ in 0..steps {
2497 let (xref, new_index) = calc_ref_trajectory(
2498 &state,
2499 &cx,
2500 &cy,
2501 &cyaw,
2502 &speed_profile,
2503 UPSTREAM_SWITCH_BACK_TICK,
2504 target_index,
2505 );
2506 target_index = new_index;
2507
2508 let mpc_result = iterative_linear_mpc_control(&xref, &state, &warm_start);
2509 state.update(mpc_result.controls[0][0], mpc_result.controls[0][1]);
2510
2511 warm_start = next_warm_start(&mpc_result.controls);
2512 }
2513
2514 SwitchBackContext {
2515 cx,
2516 cy,
2517 cyaw,
2518 speed_profile,
2519 state,
2520 target_index,
2521 warm_start,
2522 }
2523 }
2524
2525 fn collect_switch_back_prefix_trace(steps: usize) -> Vec<SwitchBackTraceRow> {
2526 let (cx, cy, cyaw) = generate_switch_back_course();
2527 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2528
2529 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2530 if state.yaw - cyaw[0] >= PI {
2531 state.yaw -= 2.0 * PI;
2532 } else if state.yaw - cyaw[0] <= -PI {
2533 state.yaw += 2.0 * PI;
2534 }
2535
2536 let mut target_index = 0;
2537 let mut warm_start = vec![Vector2::zeros(); T];
2538 let mut trace = Vec::with_capacity(steps);
2539
2540 for step in 0..steps {
2541 let (xref, new_index) = calc_ref_trajectory(
2542 &state,
2543 &cx,
2544 &cy,
2545 &cyaw,
2546 &speed_profile,
2547 UPSTREAM_SWITCH_BACK_TICK,
2548 target_index,
2549 );
2550 target_index = new_index;
2551
2552 let result = iterative_linear_mpc_control(&xref, &state, &warm_start);
2553 trace.push(SwitchBackTraceRow {
2554 step,
2555 target_index,
2556 state: state.to_vector(),
2557 xref_head: xref[0],
2558 xref_tail: *xref.last().unwrap(),
2559 control: result.controls[0],
2560 predicted_tail: *result.predicted.last().unwrap(),
2561 });
2562
2563 state.update(result.controls[0][0], result.controls[0][1]);
2564 warm_start = next_warm_start(&result.controls);
2565 }
2566
2567 trace
2568 }
2569
2570 fn collect_switch_back_detailed_trace_with_test_optimizer<F>(
2571 steps: usize,
2572 optimize: F,
2573 ) -> Vec<SwitchBackDetailedRow>
2574 where
2575 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2576 + Copy,
2577 {
2578 let (cx, cy, cyaw) = generate_switch_back_course();
2579 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2580
2581 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2582 if state.yaw - cyaw[0] >= PI {
2583 state.yaw -= 2.0 * PI;
2584 } else if state.yaw - cyaw[0] <= -PI {
2585 state.yaw += 2.0 * PI;
2586 }
2587
2588 let mut target_index = 0;
2589 let mut warm_start = vec![Vector2::zeros(); T];
2590 let mut trace = Vec::with_capacity(steps);
2591
2592 for step in 0..steps {
2593 let (xref, new_index) = calc_ref_trajectory(
2594 &state,
2595 &cx,
2596 &cy,
2597 &cyaw,
2598 &speed_profile,
2599 UPSTREAM_SWITCH_BACK_TICK,
2600 target_index,
2601 );
2602 target_index = new_index;
2603
2604 let result = iterative_linear_mpc_control_with_test_optimizer(
2605 &xref,
2606 &state,
2607 &warm_start,
2608 optimize,
2609 );
2610 trace.push(SwitchBackDetailedRow {
2611 step,
2612 target_index,
2613 state: state.to_vector(),
2614 xref: xref.clone(),
2615 controls: result.controls.clone(),
2616 predicted: result.predicted.clone(),
2617 });
2618
2619 state.update(result.controls[0][0], result.controls[0][1]);
2620 warm_start = next_warm_start(&result.controls);
2621 }
2622
2623 trace
2624 }
2625
2626 fn collect_switch_back_planning_snapshots_with_test_optimizer<F>(
2627 steps: usize,
2628 checkpoints: &[usize],
2629 optimize: F,
2630 ) -> Vec<SwitchBackPlanningSnapshot>
2631 where
2632 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2633 + Copy,
2634 {
2635 let (cx, cy, cyaw) = generate_switch_back_course();
2636 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2637
2638 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2639 if state.yaw - cyaw[0] >= PI {
2640 state.yaw -= 2.0 * PI;
2641 } else if state.yaw - cyaw[0] <= -PI {
2642 state.yaw += 2.0 * PI;
2643 }
2644
2645 let mut target_index = 0;
2646 let mut warm_start = vec![Vector2::zeros(); T];
2647 let mut trace = Vec::new();
2648
2649 for step in 0..steps {
2650 let (xref, new_index) = calc_ref_trajectory(
2651 &state,
2652 &cx,
2653 &cy,
2654 &cyaw,
2655 &speed_profile,
2656 UPSTREAM_SWITCH_BACK_TICK,
2657 target_index,
2658 );
2659 target_index = new_index;
2660
2661 if checkpoints.contains(&step) {
2662 trace.push(SwitchBackPlanningSnapshot {
2663 step,
2664 target_index,
2665 state: state.to_vector(),
2666 xref_head: xref[0],
2667 warm_start_head: warm_start.first().copied().unwrap_or_else(Vector2::zeros),
2668 });
2669 }
2670
2671 let result = iterative_linear_mpc_control_with_test_optimizer(
2672 &xref,
2673 &state,
2674 &warm_start,
2675 optimize,
2676 );
2677 state.update(result.controls[0][0], result.controls[0][1]);
2678 warm_start = next_warm_start(&result.controls);
2679 }
2680
2681 trace
2682 }
2683
2684 fn collect_switch_back_warm_start_trace_with_test_optimizer<F>(
2685 steps: usize,
2686 optimize: F,
2687 ) -> SwitchBackWarmStartTrace
2688 where
2689 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2690 + Copy,
2691 {
2692 let (cx, cy, cyaw) = generate_switch_back_course();
2693 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2694
2695 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2696 if state.yaw - cyaw[0] >= PI {
2697 state.yaw -= 2.0 * PI;
2698 } else if state.yaw - cyaw[0] <= -PI {
2699 state.yaw += 2.0 * PI;
2700 }
2701
2702 let mut target_index = 0;
2703 let mut warm_start = vec![Vector2::zeros(); T];
2704 let mut trace = SwitchBackWarmStartTrace::new();
2705
2706 for step in 0..steps {
2707 let (xref, new_index) = calc_ref_trajectory(
2708 &state,
2709 &cx,
2710 &cy,
2711 &cyaw,
2712 &speed_profile,
2713 UPSTREAM_SWITCH_BACK_TICK,
2714 target_index,
2715 );
2716 target_index = new_index;
2717 trace.insert(step, warm_start.clone());
2718
2719 let result = iterative_linear_mpc_control_with_test_optimizer(
2720 &xref,
2721 &state,
2722 &warm_start,
2723 optimize,
2724 );
2725 state.update(result.controls[0][0], result.controls[0][1]);
2726 warm_start = next_warm_start(&result.controls);
2727 }
2728
2729 trace
2730 }
2731
2732 fn collect_switch_back_detailed_trace(steps: usize) -> Vec<SwitchBackDetailedRow> {
2733 collect_switch_back_detailed_trace_with_test_optimizer(steps, optimize_linearized_controls)
2734 }
2735
2736 fn run_switch_back_simulation_with_test_optimizer<F>(
2737 max_sim_steps: usize,
2738 optimize: F,
2739 ) -> SimulationResult
2740 where
2741 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2742 + Copy,
2743 {
2744 let (cx, cy, cyaw) = generate_switch_back_course();
2745 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2746
2747 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2748 if state.yaw - cyaw[0] >= PI {
2749 state.yaw -= 2.0 * PI;
2750 } else if state.yaw - cyaw[0] <= -PI {
2751 state.yaw += 2.0 * PI;
2752 }
2753
2754 let mut target_index = 0;
2755 let final_index = cx.len() - 1;
2756 let goal = (cx[final_index], cy[final_index]);
2757 let mut warm_start = vec![Vector2::zeros(); T];
2758
2759 let mut hist_x = vec![state.x];
2760 let mut hist_y = vec![state.y];
2761 let mut predicted = vec![state.to_vector()];
2762 let mut reached_goal = false;
2763
2764 for _ in 0..max_sim_steps {
2765 let (xref, new_index) = calc_ref_trajectory(
2766 &state,
2767 &cx,
2768 &cy,
2769 &cyaw,
2770 &speed_profile,
2771 UPSTREAM_SWITCH_BACK_TICK,
2772 target_index,
2773 );
2774 target_index = new_index;
2775
2776 let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
2777 &xref,
2778 &state,
2779 &warm_start,
2780 optimize,
2781 );
2782 let accel = mpc_result.controls[0][0];
2783 let steer = mpc_result.controls[0][1];
2784
2785 state.update(accel, steer);
2786 hist_x.push(state.x);
2787 hist_y.push(state.y);
2788 predicted = mpc_result.predicted;
2789
2790 warm_start = next_warm_start(&mpc_result.controls);
2791
2792 if check_goal(&state, goal, target_index, final_index) {
2793 reached_goal = true;
2794 break;
2795 }
2796 }
2797
2798 SimulationResult {
2799 cx,
2800 cy,
2801 hist_x,
2802 hist_y,
2803 predicted_x: predicted.iter().map(|p| p[0]).collect(),
2804 predicted_y: predicted.iter().map(|p| p[1]).collect(),
2805 goal,
2806 reached_goal,
2807 target_index,
2808 final_index,
2809 final_state: state,
2810 }
2811 }
2812
2813 fn collect_switch_back_closed_loop_checkpoints_with_test_optimizer<F>(
2814 max_sim_steps: usize,
2815 checkpoints: &[usize],
2816 optimize: F,
2817 ) -> Vec<SwitchBackClosedLoopCheckpoint>
2818 where
2819 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2820 + Copy,
2821 {
2822 let (cx, cy, cyaw) = generate_switch_back_course();
2823 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2824
2825 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2826 if state.yaw - cyaw[0] >= PI {
2827 state.yaw -= 2.0 * PI;
2828 } else if state.yaw - cyaw[0] <= -PI {
2829 state.yaw += 2.0 * PI;
2830 }
2831
2832 let mut target_index = 0;
2833 let final_index = cx.len() - 1;
2834 let goal = (cx[final_index], cy[final_index]);
2835 let mut warm_start = vec![Vector2::zeros(); T];
2836 let mut recorded = Vec::new();
2837
2838 for step in 0..max_sim_steps {
2839 let (xref, new_index) = calc_ref_trajectory(
2840 &state,
2841 &cx,
2842 &cy,
2843 &cyaw,
2844 &speed_profile,
2845 UPSTREAM_SWITCH_BACK_TICK,
2846 target_index,
2847 );
2848 target_index = new_index;
2849
2850 let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
2851 &xref,
2852 &state,
2853 &warm_start,
2854 optimize,
2855 );
2856 let accel = mpc_result.controls[0][0];
2857 let steer = mpc_result.controls[0][1];
2858
2859 state.update(accel, steer);
2860 warm_start = next_warm_start(&mpc_result.controls);
2861
2862 if checkpoints.contains(&(step + 1)) {
2863 let goal_distance =
2864 ((state.x - goal.0).powi(2) + (state.y - goal.1).powi(2)).sqrt();
2865 recorded.push(SwitchBackClosedLoopCheckpoint {
2866 step: step + 1,
2867 target_index,
2868 goal_distance,
2869 state: state.to_vector(),
2870 });
2871 }
2872
2873 if check_goal(&state, goal, target_index, final_index) {
2874 break;
2875 }
2876 }
2877
2878 recorded
2879 }
2880
2881 fn collect_switch_back_goal_condition_snapshots_with_test_optimizer<F>(
2882 max_sim_steps: usize,
2883 checkpoints: &[usize],
2884 optimize: F,
2885 ) -> Vec<SwitchBackGoalConditionSnapshot>
2886 where
2887 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2888 + Copy,
2889 {
2890 let (cx, cy, cyaw) = generate_switch_back_course();
2891 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2892
2893 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2894 if state.yaw - cyaw[0] >= PI {
2895 state.yaw -= 2.0 * PI;
2896 } else if state.yaw - cyaw[0] <= -PI {
2897 state.yaw += 2.0 * PI;
2898 }
2899
2900 let mut target_index = 0;
2901 let final_index = cx.len() - 1;
2902 let goal = (cx[final_index], cy[final_index]);
2903 let mut warm_start = vec![Vector2::zeros(); T];
2904 let mut recorded = Vec::new();
2905
2906 for step in 0..max_sim_steps {
2907 let (xref, new_index) = calc_ref_trajectory(
2908 &state,
2909 &cx,
2910 &cy,
2911 &cyaw,
2912 &speed_profile,
2913 UPSTREAM_SWITCH_BACK_TICK,
2914 target_index,
2915 );
2916 target_index = new_index;
2917
2918 let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
2919 &xref,
2920 &state,
2921 &warm_start,
2922 optimize,
2923 );
2924 let accel = mpc_result.controls[0][0];
2925 let steer = mpc_result.controls[0][1];
2926
2927 state.update(accel, steer);
2928 warm_start = next_warm_start(&mpc_result.controls);
2929
2930 let dx = state.x - goal.0;
2931 let dy = state.y - goal.1;
2932 let goal_distance = (dx * dx + dy * dy).sqrt();
2933 let heading_error = angle_diff(state.yaw, cyaw[target_index]);
2934 let motion_curvature = steer.tan() / WB;
2935 let near_goal = goal_distance <= GOAL_DIS;
2936 let near_end = final_index.abs_diff(target_index) < 5;
2937 let stopped = state.v.abs() <= STOP_SPEED;
2938 let reached_goal = near_goal && near_end && stopped;
2939
2940 if checkpoints.contains(&(step + 1)) {
2941 recorded.push(SwitchBackGoalConditionSnapshot {
2942 step: step + 1,
2943 target_index,
2944 goal_distance,
2945 speed: state.v,
2946 speed_abs: state.v.abs(),
2947 heading_error,
2948 motion_curvature,
2949 near_goal,
2950 near_end,
2951 stopped,
2952 reached_goal,
2953 });
2954 }
2955
2956 if reached_goal {
2957 break;
2958 }
2959 }
2960
2961 recorded
2962 }
2963
2964 fn collect_switch_back_goal_control_snapshots_with_test_optimizer<F>(
2965 max_sim_steps: usize,
2966 checkpoints: &[usize],
2967 optimize: F,
2968 ) -> Vec<SwitchBackGoalControlSnapshot>
2969 where
2970 F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2971 + Copy,
2972 {
2973 let (cx, cy, cyaw) = generate_switch_back_course();
2974 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2975
2976 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2977 if state.yaw - cyaw[0] >= PI {
2978 state.yaw -= 2.0 * PI;
2979 } else if state.yaw - cyaw[0] <= -PI {
2980 state.yaw += 2.0 * PI;
2981 }
2982
2983 let mut target_index = 0;
2984 let final_index = cx.len() - 1;
2985 let goal = (cx[final_index], cy[final_index]);
2986 let mut warm_start = vec![Vector2::zeros(); T];
2987 let mut recorded = Vec::new();
2988
2989 for step in 0..max_sim_steps {
2990 let (xref, new_index) = calc_ref_trajectory(
2991 &state,
2992 &cx,
2993 &cy,
2994 &cyaw,
2995 &speed_profile,
2996 UPSTREAM_SWITCH_BACK_TICK,
2997 target_index,
2998 );
2999 target_index = new_index;
3000
3001 let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
3002 &xref,
3003 &state,
3004 &warm_start,
3005 optimize,
3006 );
3007 let accel = mpc_result.controls[0][0];
3008 let steer = mpc_result.controls[0][1];
3009
3010 state.update(accel, steer);
3011 warm_start = next_warm_start(&mpc_result.controls);
3012
3013 let dx = state.x - goal.0;
3014 let dy = state.y - goal.1;
3015 let goal_distance = (dx * dx + dy * dy).sqrt();
3016 let heading_error = angle_diff(state.yaw, cyaw[target_index]);
3017 let motion_curvature = steer.tan() / WB;
3018 let course_curvature = course_curvature_at_index(&cx, &cy, &cyaw, target_index);
3019 let near_goal = goal_distance <= GOAL_DIS;
3020 let near_end = final_index.abs_diff(target_index) < 5;
3021 let stopped = state.v.abs() <= STOP_SPEED;
3022 let reached_goal = near_goal && near_end && stopped;
3023
3024 if checkpoints.contains(&(step + 1)) {
3025 recorded.push(SwitchBackGoalControlSnapshot {
3026 step: step + 1,
3027 target_index,
3028 goal_distance,
3029 speed: state.v,
3030 accel,
3031 steer,
3032 heading_error,
3033 motion_curvature,
3034 course_curvature,
3035 near_goal,
3036 near_end,
3037 stopped,
3038 reached_goal,
3039 });
3040 }
3041
3042 if reached_goal {
3043 break;
3044 }
3045 }
3046
3047 recorded
3048 }
3049
3050 fn goal_distance_delta(
3051 default: &SwitchBackClosedLoopCheckpoint,
3052 expanded: &SwitchBackClosedLoopCheckpoint,
3053 ) -> f64 {
3054 default.goal_distance - expanded.goal_distance
3055 }
3056
3057 fn checkpoint_at_step(
3058 checkpoints: &[SwitchBackClosedLoopCheckpoint],
3059 step: usize,
3060 ) -> &SwitchBackClosedLoopCheckpoint {
3061 checkpoints
3062 .iter()
3063 .find(|checkpoint| checkpoint.step == step)
3064 .unwrap_or_else(|| panic!("missing checkpoint for step {}", step))
3065 }
3066
3067 fn goal_distance_slope(
3068 start: &SwitchBackClosedLoopCheckpoint,
3069 end: &SwitchBackClosedLoopCheckpoint,
3070 ) -> f64 {
3071 let span = (end.step - start.step) as f64;
3072 (start.goal_distance - end.goal_distance) / span
3073 }
3074
3075 fn target_index_increment(
3076 start: &SwitchBackClosedLoopCheckpoint,
3077 end: &SwitchBackClosedLoopCheckpoint,
3078 ) -> isize {
3079 end.target_index as isize - start.target_index as isize
3080 }
3081
3082 fn checkpoint_lateral_error(
3083 checkpoint: &SwitchBackClosedLoopCheckpoint,
3084 cx: &[f64],
3085 cy: &[f64],
3086 cyaw: &[f64],
3087 ) -> f64 {
3088 let index = checkpoint.target_index.min(cx.len().saturating_sub(1));
3089 let dx = checkpoint.state[0] - cx[index];
3090 let dy = checkpoint.state[1] - cy[index];
3091 -dx * cyaw[index].sin() + dy * cyaw[index].cos()
3092 }
3093
3094 fn checkpoint_heading_error(checkpoint: &SwitchBackClosedLoopCheckpoint, cyaw: &[f64]) -> f64 {
3095 let index = checkpoint.target_index.min(cyaw.len().saturating_sub(1));
3096 angle_diff(checkpoint.state[3], cyaw[index])
3097 }
3098
3099 fn course_curvature_at_index(cx: &[f64], cy: &[f64], cyaw: &[f64], index: usize) -> f64 {
3100 if cx.len() < 3 || cy.len() < 3 || cyaw.len() < 3 {
3101 return 0.0;
3102 }
3103
3104 let i = index.clamp(1, cx.len().saturating_sub(2));
3105 let ds_prev = ((cx[i] - cx[i - 1]).powi(2) + (cy[i] - cy[i - 1]).powi(2))
3106 .sqrt()
3107 .max(f64::EPSILON);
3108 let ds_next = ((cx[i + 1] - cx[i]).powi(2) + (cy[i + 1] - cy[i]).powi(2))
3109 .sqrt()
3110 .max(f64::EPSILON);
3111
3112 angle_diff(cyaw[i + 1], cyaw[i - 1]) / (ds_prev + ds_next)
3113 }
3114
3115 fn scalar_slope(start: f64, end: f64, steps: usize) -> f64 {
3116 (end - start) / steps as f64
3117 }
3118
3119 fn goal_snapshot_at_step(
3120 snapshots: &[SwitchBackGoalConditionSnapshot],
3121 step: usize,
3122 ) -> &SwitchBackGoalConditionSnapshot {
3123 snapshots
3124 .iter()
3125 .find(|snapshot| snapshot.step == step)
3126 .unwrap_or_else(|| panic!("missing goal-condition snapshot for step {}", step))
3127 }
3128
3129 fn first_goal_condition_step<F>(
3130 snapshots: &[SwitchBackGoalConditionSnapshot],
3131 predicate: F,
3132 ) -> Option<usize>
3133 where
3134 F: Fn(&SwitchBackGoalConditionSnapshot) -> bool,
3135 {
3136 snapshots
3137 .iter()
3138 .find(|snapshot| predicate(snapshot))
3139 .map(|s| s.step)
3140 }
3141
3142 fn goal_control_snapshot_at_step(
3143 snapshots: &[SwitchBackGoalControlSnapshot],
3144 step: usize,
3145 ) -> &SwitchBackGoalControlSnapshot {
3146 snapshots
3147 .iter()
3148 .find(|snapshot| snapshot.step == step)
3149 .unwrap_or_else(|| panic!("missing goal-control snapshot for step {}", step))
3150 }
3151
3152 fn first_goal_control_condition_step<F>(
3153 snapshots: &[SwitchBackGoalControlSnapshot],
3154 predicate: F,
3155 ) -> Option<usize>
3156 where
3157 F: Fn(&SwitchBackGoalControlSnapshot) -> bool,
3158 {
3159 snapshots
3160 .iter()
3161 .find(|snapshot| predicate(snapshot))
3162 .map(|snapshot| snapshot.step)
3163 }
3164
3165 fn detailed_row_at_step(rows: &[SwitchBackDetailedRow], step: usize) -> &SwitchBackDetailedRow {
3166 rows.iter()
3167 .find(|row| row.step == step)
3168 .unwrap_or_else(|| panic!("missing detailed row for step {}", step))
3169 }
3170
3171 fn planning_snapshot_at_step(
3172 snapshots: &[SwitchBackPlanningSnapshot],
3173 step: usize,
3174 ) -> &SwitchBackPlanningSnapshot {
3175 snapshots
3176 .iter()
3177 .find(|snapshot| snapshot.step == step)
3178 .unwrap_or_else(|| panic!("missing planning snapshot for step {}", step))
3179 }
3180
3181 fn assert_planning_snapshot_phase_alignment(
3182 label: &str,
3183 default: &SwitchBackPlanningSnapshot,
3184 expanded: &SwitchBackPlanningSnapshot,
3185 ) {
3186 assert_eq!(
3187 default.target_index, expanded.target_index,
3188 "{} target-index alignment moved unexpectedly: default={:?} expanded={:?}",
3189 label, default, expanded
3190 );
3191 assert_diff_bounds(
3192 &format!("{label}_state"),
3193 trace_state_diff(&default.state, &expanded.state),
3194 [2e-4, 2e-4, 1e-4, 1e-4],
3195 );
3196 assert_diff_bounds(
3197 &format!("{label}_xref_head"),
3198 trace_state_diff(&default.xref_head, &expanded.xref_head),
3199 [1e-10; 4],
3200 );
3201 assert_diff_bounds(
3202 &format!("{label}_warm_start"),
3203 trace_control_diff(&default.warm_start_head, &expanded.warm_start_head),
3204 [1e-10, 3e-4],
3205 );
3206 }
3207
3208 fn assert_goal_control_alignment(
3209 default: &SwitchBackGoalControlSnapshot,
3210 expanded: &SwitchBackGoalControlSnapshot,
3211 ) {
3212 assert_eq!(default.target_index, expanded.target_index);
3213 assert_eq!(default.near_goal, expanded.near_goal);
3214 assert_eq!(default.near_end, expanded.near_end);
3215 assert_eq!(default.stopped, expanded.stopped);
3216 assert_eq!(default.reached_goal, expanded.reached_goal);
3217 assert!(
3218 (default.goal_distance - expanded.goal_distance).abs() <= 3.0e-4,
3219 "goal distance alignment moved unexpectedly: default={:?} expanded={:?}",
3220 default,
3221 expanded
3222 );
3223 assert!(
3224 (default.speed - expanded.speed).abs() <= 3.0e-4,
3225 "speed alignment moved unexpectedly: default={:?} expanded={:?}",
3226 default,
3227 expanded
3228 );
3229 assert!(
3230 (default.accel - expanded.accel).abs() <= 2.0e-3,
3231 "accel alignment moved unexpectedly: default={:?} expanded={:?}",
3232 default,
3233 expanded
3234 );
3235 assert!(
3236 (default.steer - expanded.steer).abs() <= 1.0e-4,
3237 "steer alignment moved unexpectedly: default={:?} expanded={:?}",
3238 default,
3239 expanded
3240 );
3241 assert!(
3242 (default.heading_error - expanded.heading_error).abs() <= 5.0e-5,
3243 "heading-error alignment moved unexpectedly: default={:?} expanded={:?}",
3244 default,
3245 expanded
3246 );
3247 assert!(
3248 (default.motion_curvature - expanded.motion_curvature).abs() <= 5.0e-5,
3249 "motion-curvature alignment moved unexpectedly: default={:?} expanded={:?}",
3250 default,
3251 expanded
3252 );
3253 assert!(
3254 (default.course_curvature - expanded.course_curvature).abs() <= 1.0e-12,
3255 "course-curvature alignment moved unexpectedly: default={:?} expanded={:?}",
3256 default,
3257 expanded
3258 );
3259 }
3260
3261 fn assert_detailed_phase_alignment(
3262 label: &str,
3263 default: &SwitchBackDetailedRow,
3264 expanded: &SwitchBackDetailedRow,
3265 ) {
3266 assert_eq!(
3267 default.target_index, expanded.target_index,
3268 "{} target-index alignment moved unexpectedly: default={:?} expanded={:?}",
3269 label, default, expanded
3270 );
3271
3272 let expanded_xref_rows = expanded
3273 .xref
3274 .iter()
3275 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3276 .collect::<Vec<_>>();
3277 let expanded_control_rows = expanded
3278 .controls
3279 .iter()
3280 .map(|vec| [vec[0], vec[1]])
3281 .collect::<Vec<_>>();
3282 let expanded_predicted_rows = expanded
3283 .predicted
3284 .iter()
3285 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3286 .collect::<Vec<_>>();
3287
3288 assert_diff_bounds(
3289 &format!("{label}_state"),
3290 trace_state_diff(&default.state, &expanded.state),
3291 [5e-4, 5e-4, 5e-4, 2e-4],
3292 );
3293 assert_diff_bounds(
3294 &format!("{label}_xref"),
3295 summarize_vec4_sequence_diff(&default.xref, &expanded_xref_rows),
3296 [1e-10; 4],
3297 );
3298 assert_diff_bounds(
3299 &format!("{label}_controls"),
3300 summarize_vec2_sequence_diff(&default.controls, &expanded_control_rows),
3301 [2e-3, 2e-4],
3302 );
3303 assert_diff_bounds(
3304 &format!("{label}_predicted"),
3305 summarize_vec4_sequence_diff(&default.predicted, &expanded_predicted_rows),
3306 [7e-4, 7e-4, 7e-4, 2e-4],
3307 );
3308 }
3309
3310 #[test]
3311 fn test_speed_profile_stops_at_goal() {
3312 let cx = vec![0.0, 1.0, 2.0];
3313 let cy = vec![0.0, 0.0, 0.0];
3314 let cyaw = vec![0.0, 0.0, 0.0];
3315 let speed = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
3316
3317 assert_eq!(speed.len(), 3);
3318 assert_eq!(speed[2], 0.0);
3319 assert!(speed[0] > 0.0);
3320 }
3321
3322 #[test]
3323 fn test_iterative_linear_mpc_matches_reference_offset_state() {
3324 let xref = vec![
3327 Vector4::new(0.0, 0.0, TARGET_SPEED, 0.0),
3328 Vector4::new(0.5, 0.0, TARGET_SPEED, 0.0),
3329 Vector4::new(1.0, 0.0, TARGET_SPEED, 0.0),
3330 Vector4::new(1.5, 0.0, TARGET_SPEED, 0.0),
3331 Vector4::new(2.0, 0.0, TARGET_SPEED, 0.0),
3332 Vector4::new(2.5, 0.0, TARGET_SPEED, 0.0),
3333 ];
3334 let state = State::new(0.0, 0.3, 1.0, 0.1);
3335
3336 let result = iterative_linear_mpc_control(&xref, &state, &[]);
3337
3338 let expected_controls = [
3339 [1.0, -0.669_667_215_071],
3340 [1.0, -0.630_794_908_167],
3341 [1.0, -0.577_878_962_843],
3342 [1.0, -0.534_142_026_172],
3343 [1.0, -0.513_091_902_297],
3344 ];
3345 for (actual, expected) in result.controls.iter().zip(expected_controls.iter()) {
3346 assert_vec2_close(actual, *expected, 5e-3);
3347 }
3348
3349 let final_predicted = result.predicted.last().unwrap();
3350 assert_vec4_close(
3351 final_predicted,
3352 [1.394_401_496_574, 0.249_081_964_95, 2.0, -0.221_107_545_509],
3353 5e-2,
3354 );
3355 }
3356
3357 #[test]
3358 fn test_mpc_simulation_reaches_goal() {
3359 let ax = vec![0.0, 6.0, 12.0];
3360 let ay = vec![0.0, 0.0, 0.0];
3361 let result = run_mpc_simulation_with_course(&ax, &ay, 120);
3362 let goal_distance = ((result.final_state.x - result.goal.0).powi(2)
3363 + (result.final_state.y - result.goal.1).powi(2))
3364 .sqrt();
3365
3366 assert!(result.reached_goal);
3367 assert!(goal_distance <= GOAL_DIS + 0.25);
3368 assert!(result.hist_x.len() < 120);
3369 }
3370
3371 #[test]
3372 fn test_mpc_straight_course_closed_loop_matches_reference() {
3373 let ax = vec![0.0, 6.0, 12.0];
3376 let ay = vec![0.0, 0.0, 0.0];
3377 let result = run_mpc_simulation_with_course(&ax, &ay, 120);
3378
3379 assert!(result.reached_goal);
3380 assert_eq!(result.target_index, result.final_index);
3381 assert_eq!(result.target_index, 24);
3382 assert_eq!(result.hist_x.len(), 56);
3383
3384 assert_vec4_close(
3385 &result.final_state.to_vector(),
3386 [
3387 12.126_756_016_762_242,
3388 0.000_001_043_289_690_068_484_6,
3389 -0.123_832_283_912_188_83,
3390 0.000_000_381_531_819_982_818_5,
3391 ],
3392 1e-2,
3393 );
3394 }
3395
3396 #[test]
3397 fn test_switch_back_course_contains_reverse_segment() {
3398 let (cx, cy, cyaw) = generate_switch_back_course();
3399 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
3400
3401 assert_eq!(cx.first().copied(), Some(0.0));
3402 assert_eq!(cy.first().copied(), Some(0.0));
3403 assert_eq!(cx.len(), 162);
3404 assert!(cy.iter().copied().fold(f64::NEG_INFINITY, f64::max) > 30.0);
3405 assert!((cx.last().unwrap() + 0.016_802_167_501_358_26).abs() <= 1e-9);
3406 assert!((cy.last().unwrap() - 0.833_302_319_727_219_5).abs() <= 1e-9);
3407 assert!(speed_profile[..speed_profile.len().saturating_sub(1)]
3408 .iter()
3409 .any(|speed| *speed < 0.0));
3410 assert_eq!(speed_profile.last().copied(), Some(0.0));
3411 }
3412
3413 #[test]
3414 fn test_mpc_switch_back_midcourse_window_matches_reference() {
3415 let ctx = advance_switch_back_context(20);
3418 let (xref, target_index) = calc_ref_trajectory(
3419 &ctx.state,
3420 &ctx.cx,
3421 &ctx.cy,
3422 &ctx.cyaw,
3423 &ctx.speed_profile,
3424 UPSTREAM_SWITCH_BACK_TICK,
3425 ctx.target_index,
3426 );
3427 let result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
3428
3429 assert_eq!(target_index, 5);
3430 assert_vec4_close(
3431 &ctx.state.to_vector(),
3432 [
3433 7.528_990_211_684_919,
3434 -0.616_644_989_009_243_4,
3435 3.855_895_792_930_033_5,
3436 -0.078_281_934_138_131_24,
3437 ],
3438 1e-2,
3439 );
3440
3441 let expected_xref = [
3442 [
3443 7.884_351_723_374_65,
3444 -0.646_692_295_268_678_9,
3445 TARGET_SPEED,
3446 -0.078_822_806_954_514_1,
3447 ],
3448 [
3449 9.417_709_562_051_241,
3450 -0.766_274_939_125_219_6,
3451 TARGET_SPEED,
3452 -0.076_759_688_257_237_88,
3453 ],
3454 [
3455 10.927_333_306_546_922,
3456 -0.880_536_229_237_835_5,
3457 TARGET_SPEED,
3458 -0.074_243_230_035_395_7,
3459 ],
3460 [
3461 10.927_333_306_546_922,
3462 -0.880_536_229_237_835_5,
3463 TARGET_SPEED,
3464 -0.074_243_230_035_395_7,
3465 ],
3466 [
3467 12.409_267_274_498_205,
3468 -0.988_589_273_315_872_4,
3469 TARGET_SPEED,
3470 -0.071_228_758_450_065_36,
3471 ],
3472 [
3473 13.859_555_783_541_607,
3474 -1.089_547_179_068_676,
3475 TARGET_SPEED,
3476 -0.067_659_809_911_111_4,
3477 ],
3478 ];
3479 for (actual, expected) in xref.iter().zip(expected_xref.iter()) {
3480 assert_vec4_close(actual, *expected, 1e-3);
3481 }
3482
3483 let expected_controls = [
3484 [0.999_999_995_100_728_5, 0.004_502_534_498_214_272],
3485 [0.619_401_970_887_073, 0.003_626_365_725_009_755],
3486 [-0.999_999_586_012_278, 0.003_497_926_790_795_169],
3487 [-0.999_999_988_263_528_5, 0.004_178_759_548_203_836],
3488 [-0.999_999_985_187_766_1, 0.004_753_969_705_983_846],
3489 ];
3490 for (actual, expected) in result.controls.iter().zip(expected_controls.iter()) {
3491 assert_vec2_close(actual, *expected, 2e-2);
3492 }
3493
3494 let final_predicted = result.predicted.last().unwrap();
3495 assert_vec4_close(
3496 final_predicted,
3497 [
3498 11.487_827_888_938_794,
3499 -0.917_080_324_028_499_6,
3500 3.579_776_274_235_015_4,
3501 -0.071_778_774_991_017_19,
3502 ],
3503 5e-2,
3504 );
3505 }
3506
3507 #[test]
3508 fn test_mpc_switch_back_prefix_trace_matches_pythonrobotics_reference() {
3509 let expected =
3510 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3511 let strict_prefix_steps = 71;
3515 let expected = &expected[..strict_prefix_steps];
3516 let actual = collect_switch_back_prefix_trace(expected.len());
3517 let summary = summarize_switch_back_trace_window(&actual, expected);
3518
3519 assert_eq!(summary.max_target_index_gap, 0);
3520 assert_diff_bounds("state", summary.max_state_diff, [4e-3, 3e-3, 4e-3, 2e-3]);
3521 assert_diff_bounds("xref_head", summary.max_xref_head_diff, [1e-10; 4]);
3522 assert_diff_bounds("xref_tail", summary.max_xref_tail_diff, [1e-10; 4]);
3523 assert_diff_bounds("control", summary.max_control_diff, [1e-2, 6e-3]);
3524 assert_diff_bounds(
3525 "predicted_tail",
3526 summary.max_predicted_tail_diff,
3527 [0.11, 0.13, 5e-3, 0.17],
3528 );
3529 }
3530
3531 #[test]
3532 fn test_mpc_switch_back_reverse_transition_matches_pythonrobotics_reference() {
3533 let expected =
3534 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3535 let reverse_window = 71..81;
3539 let actual = collect_switch_back_prefix_trace(reverse_window.end);
3540 let summary = summarize_switch_back_trace_window(
3541 &actual[reverse_window.clone()],
3542 &expected[reverse_window],
3543 );
3544
3545 assert!(summary.max_target_index_gap <= 1);
3546 assert_diff_bounds(
3547 "reverse_state",
3548 summary.max_state_diff,
3549 [0.04, 0.35, 0.17, 0.2],
3550 );
3551 assert_diff_bounds(
3552 "reverse_xref_head",
3553 summary.max_xref_head_diff,
3554 [0.75, 0.45, 1e-12, 0.02],
3555 );
3556 assert_diff_bounds(
3557 "reverse_xref_tail",
3558 summary.max_xref_tail_diff,
3559 [0.95, 0.5, 1e-12, 0.01],
3560 );
3561 assert_diff_bounds("reverse_control", summary.max_control_diff, [0.86, 0.41]);
3562 assert_diff_bounds(
3563 "reverse_predicted_tail",
3564 summary.max_predicted_tail_diff,
3565 [0.26, 0.62, 0.36, 0.55],
3566 );
3567 }
3568
3569 #[test]
3570 fn test_mpc_switch_back_reverse_detailed_trace_matches_pythonrobotics_reference() {
3571 let expected_trace =
3572 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3573 let expected_controls = parse_switch_back_reverse_controls(include_str!(
3574 "testdata/mpc_switch_back_reverse_controls.csv"
3575 ));
3576 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3577 "testdata/mpc_switch_back_reverse_horizon.csv"
3578 ));
3579 let actual_trace = collect_switch_back_detailed_trace(81);
3580 let summary = summarize_switch_back_detailed_window(
3581 &actual_trace,
3582 &expected_trace,
3583 &expected_controls,
3584 &expected_horizon,
3585 71..=80,
3586 );
3587
3588 assert!(summary.max_target_index_gap <= 1);
3589 assert_diff_bounds(
3590 "reverse_detailed_state",
3591 summary.max_state_diff,
3592 [0.04, 0.35, 0.17, 0.21],
3593 );
3594 assert_diff_bounds(
3595 "reverse_detailed_xref",
3596 summary.max_xref_diff,
3597 [0.92, 0.49, 1e-12, 0.02],
3598 );
3599 assert_diff_bounds(
3600 "reverse_detailed_control",
3601 summary.max_control_diff,
3602 [1.46, 0.5],
3603 );
3604 assert_diff_bounds(
3605 "reverse_detailed_predicted",
3606 summary.max_predicted_diff,
3607 [0.26, 0.62, 0.47, 0.55],
3608 );
3609 }
3610
3611 #[test]
3612 fn test_mpc_switch_back_reverse_full_candidate_expansion_characterizes_detailed_trace_tradeoff()
3613 {
3614 let expected_trace =
3615 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3616 let expected_controls = parse_switch_back_reverse_controls(include_str!(
3617 "testdata/mpc_switch_back_reverse_controls.csv"
3618 ));
3619 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3620 "testdata/mpc_switch_back_reverse_horizon.csv"
3621 ));
3622 let actual_trace = collect_switch_back_detailed_trace_with_test_optimizer(
3623 81,
3624 optimize_linearized_controls_with_full_candidate_expansion,
3625 );
3626 let default_summary = summarize_switch_back_detailed_window(
3627 &collect_switch_back_detailed_trace(81),
3628 &expected_trace,
3629 &expected_controls,
3630 &expected_horizon,
3631 71..=80,
3632 );
3633 let expanded_summary = summarize_switch_back_detailed_window(
3634 &actual_trace,
3635 &expected_trace,
3636 &expected_controls,
3637 &expected_horizon,
3638 71..=80,
3639 );
3640
3641 assert!(expanded_summary.max_target_index_gap <= 1);
3642 assert_eq!(
3643 expanded_summary.max_xref_diff,
3644 default_summary.max_xref_diff
3645 );
3646 assert_diff_bounds(
3647 "reverse_detailed_full_candidates_state_characterization",
3648 expanded_summary.max_state_diff,
3649 [0.043, 0.35, 0.17, 0.21],
3650 );
3651 assert_diff_bounds(
3652 "reverse_detailed_full_candidates_control_characterization",
3653 expanded_summary.max_control_diff,
3654 [1.32, 0.5],
3655 );
3656 assert_diff_bounds(
3657 "reverse_detailed_full_candidates_predicted_characterization",
3658 expanded_summary.max_predicted_diff,
3659 [0.22, 0.62, 0.41, 0.43],
3660 );
3661
3662 for (i, (d, e)) in default_summary
3666 .max_state_diff
3667 .iter()
3668 .zip(expanded_summary.max_state_diff.iter())
3669 .enumerate()
3670 {
3671 assert!(
3672 (d - e).abs() < 0.2,
3673 "state diff index {} diverged too much: default={} expanded={}",
3674 i,
3675 d,
3676 e
3677 );
3678 }
3679 for (i, (d, e)) in default_summary
3680 .max_control_diff
3681 .iter()
3682 .zip(expanded_summary.max_control_diff.iter())
3683 .enumerate()
3684 {
3685 assert!(
3686 (d - e).abs() < 0.2,
3687 "control diff index {} diverged too much: default={} expanded={}",
3688 i,
3689 d,
3690 e
3691 );
3692 }
3693 }
3694
3695 #[test]
3696 fn test_mpc_switch_back_reverse_solver_gap_with_python_warm_start() {
3697 let expected_trace =
3698 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3699 let expected_controls = parse_switch_back_reverse_controls(include_str!(
3700 "testdata/mpc_switch_back_reverse_controls.csv"
3701 ));
3702 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3703 "testdata/mpc_switch_back_reverse_horizon.csv"
3704 ));
3705
3706 for step in [75usize, 78usize] {
3707 let expected_state = &expected_trace[step].state;
3708 let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3709 let expected_controls_for_step = expected_controls.get(&step).unwrap();
3710 let (expected_xref_for_step, expected_predicted_for_step) =
3711 expected_horizon.get(&step).unwrap();
3712
3713 let expected_control_rows = expected_controls_for_step
3714 .iter()
3715 .map(|vec| [vec[0], vec[1]])
3716 .collect::<Vec<_>>();
3717 let expected_predicted_rows = expected_predicted_for_step
3718 .iter()
3719 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3720 .collect::<Vec<_>>();
3721
3722 let result = iterative_linear_mpc_control(
3723 expected_xref_for_step,
3724 &state_from_vector4(expected_state),
3725 python_warm_start,
3726 );
3727
3728 let control_diff =
3729 summarize_vec2_sequence_diff(&result.controls, &expected_control_rows);
3730 let predicted_diff =
3731 summarize_vec4_sequence_diff(&result.predicted, &expected_predicted_rows);
3732 let (control_tol, predicted_tol) = match step {
3733 75 => ([0.38, 0.11], [0.09, 0.23, 0.11, 0.18]),
3734 78 => ([0.52, 0.35], [0.03, 0.46, 0.11, 0.25]),
3735 _ => unreachable!(),
3736 };
3737
3738 assert_diff_bounds(
3739 &format!("python_warm_start_step_{step}_control"),
3740 control_diff,
3741 control_tol,
3742 );
3743 assert_diff_bounds(
3744 &format!("python_warm_start_step_{step}_predicted"),
3745 predicted_diff,
3746 predicted_tol,
3747 );
3748 }
3749 }
3750
3751 #[test]
3752 fn test_mpc_switch_back_predict_motion_matches_python_warm_start_reference() {
3753 let expected_trace =
3754 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3755 let expected_controls = parse_switch_back_reverse_controls(include_str!(
3756 "testdata/mpc_switch_back_reverse_controls.csv"
3757 ));
3758 let expected_xbar = parse_switch_back_reverse_states(include_str!(
3759 "testdata/mpc_switch_back_reverse_xbar.csv"
3760 ));
3761
3762 for step in [75usize, 78usize] {
3763 let expected_state = &expected_trace[step].state;
3764 let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3765 let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
3766 let actual_xbar = predict_motion(state_from_vector4(expected_state), python_warm_start);
3767 let xbar_diff = summarize_vec4_sequence_diff(
3768 &actual_xbar,
3769 &expected_xbar_for_step
3770 .iter()
3771 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3772 .collect::<Vec<_>>(),
3773 );
3774
3775 assert_diff_bounds(
3776 &format!("python_warm_start_step_{step}_xbar"),
3777 xbar_diff,
3778 [1e-9, 1e-9, 1e-9, 1e-9],
3779 );
3780 }
3781 }
3782
3783 #[test]
3784 fn test_mpc_switch_back_reverse_linearized_cost_gap_against_python_reference() {
3785 let expected_trace =
3786 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3787 let expected_controls = parse_switch_back_reverse_controls(include_str!(
3788 "testdata/mpc_switch_back_reverse_controls.csv"
3789 ));
3790 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3791 "testdata/mpc_switch_back_reverse_horizon.csv"
3792 ));
3793 let expected_xbar = parse_switch_back_reverse_states(include_str!(
3794 "testdata/mpc_switch_back_reverse_xbar.csv"
3795 ));
3796
3797 for step in [75usize, 78usize] {
3798 let expected_state = &expected_trace[step].state;
3799 let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3800 let python_controls = expected_controls.get(&step).unwrap();
3801 let (expected_xref_for_step, expected_predicted_for_step) =
3802 expected_horizon.get(&step).unwrap();
3803 let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
3804 let state = state_from_vector4(expected_state);
3805
3806 let rust_controls = optimize_linearized_controls(
3807 expected_xref_for_step,
3808 expected_xbar_for_step,
3809 &state,
3810 python_warm_start,
3811 );
3812 let python_x_python_style = linearized_rollout_without_angle_wrap(
3813 &state,
3814 expected_xbar_for_step,
3815 python_controls,
3816 );
3817 let python_cost_python_style = compute_cost_without_angle_wrap(
3818 &python_x_python_style,
3819 expected_xref_for_step,
3820 python_controls,
3821 );
3822
3823 let rust_x_python_style = linearized_rollout_without_angle_wrap(
3824 &state,
3825 expected_xbar_for_step,
3826 &rust_controls,
3827 );
3828 let rust_cost_python_style = compute_cost_without_angle_wrap(
3829 &rust_x_python_style,
3830 expected_xref_for_step,
3831 &rust_controls,
3832 );
3833 let predicted_diff_python_style = summarize_vec4_sequence_diff(
3834 &python_x_python_style,
3835 &expected_predicted_for_step
3836 .iter()
3837 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3838 .collect::<Vec<_>>(),
3839 );
3840 let predicted_tol = match step {
3841 75 => [0.11, 0.07, 1e-12, 0.002],
3842 78 => [0.06, 0.02, 1e-12, 0.006],
3843 _ => unreachable!(),
3844 };
3845
3846 assert_diff_bounds(
3847 &format!("python_style_linearized_rollout_step_{step}"),
3848 predicted_diff_python_style,
3849 predicted_tol,
3850 );
3851
3852 let solver_gap = rust_cost_python_style - python_cost_python_style;
3853 let min_gap = match step {
3854 75 => 1.0,
3855 78 => -0.1,
3857 _ => unreachable!(),
3858 };
3859 assert!(
3860 solver_gap > min_gap,
3861 "step {} solver gap too small: rust={} python={} gap={}",
3862 step,
3863 rust_cost_python_style,
3864 python_cost_python_style,
3865 solver_gap
3866 );
3867 }
3868 }
3869
3870 #[test]
3871 fn test_mpc_switch_back_reverse_python_style_solver_trace() {
3872 let expected_trace =
3873 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3874 let expected_controls = parse_switch_back_reverse_controls(include_str!(
3875 "testdata/mpc_switch_back_reverse_controls.csv"
3876 ));
3877 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3878 "testdata/mpc_switch_back_reverse_horizon.csv"
3879 ));
3880 let expected_xbar = parse_switch_back_reverse_states(include_str!(
3881 "testdata/mpc_switch_back_reverse_xbar.csv"
3882 ));
3883
3884 for step in [75usize, 78usize] {
3885 let expected_state = &expected_trace[step].state;
3886 let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3887 let python_controls = expected_controls.get(&step).unwrap();
3888 let (expected_xref_for_step, _) = expected_horizon.get(&step).unwrap();
3889 let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
3890 let state = state_from_vector4(expected_state);
3891
3892 let (default_controls, default_trace) =
3893 optimize_linearized_controls_without_angle_wrap_with_trace(
3894 expected_xref_for_step,
3895 expected_xbar_for_step,
3896 &state,
3897 python_warm_start,
3898 QP_MAX_ITERS,
3899 LINE_SEARCH_ITERS,
3900 );
3901 let (extended_controls, extended_trace) =
3902 optimize_linearized_controls_without_angle_wrap_with_trace(
3903 expected_xref_for_step,
3904 expected_xbar_for_step,
3905 &state,
3906 python_warm_start,
3907 QP_MAX_ITERS * 10,
3908 LINE_SEARCH_ITERS * 2,
3909 );
3910
3911 let python_x = linearized_rollout_without_angle_wrap(
3912 &state,
3913 expected_xbar_for_step,
3914 python_controls,
3915 );
3916 let python_cost =
3917 compute_cost_without_angle_wrap(&python_x, expected_xref_for_step, python_controls);
3918
3919 let default_x = linearized_rollout_without_angle_wrap(
3920 &state,
3921 expected_xbar_for_step,
3922 &default_controls,
3923 );
3924 let default_cost = compute_cost_without_angle_wrap(
3925 &default_x,
3926 expected_xref_for_step,
3927 &default_controls,
3928 );
3929
3930 let extended_x = linearized_rollout_without_angle_wrap(
3931 &state,
3932 expected_xbar_for_step,
3933 &extended_controls,
3934 );
3935 let extended_cost = compute_cost_without_angle_wrap(
3936 &extended_x,
3937 expected_xref_for_step,
3938 &extended_controls,
3939 );
3940
3941 let default_gap = default_cost - python_cost;
3942 let extended_gap = extended_cost - python_cost;
3943 let last_default_grad = default_trace
3944 .last()
3945 .map(|entry| entry.gradient_norm)
3946 .unwrap_or(0.0);
3947 let last_extended_grad = extended_trace
3948 .last()
3949 .map(|entry| entry.gradient_norm)
3950 .unwrap_or(0.0);
3951
3952 assert!(
3953 extended_cost <= default_cost + 1e-9,
3954 "step {} extended cost regressed: default={} extended={}",
3955 step,
3956 default_cost,
3957 extended_cost
3958 );
3959 assert!(
3960 default_trace
3961 .iter()
3962 .any(|entry| entry.accepted_step.is_some()),
3963 "step {} default trace never accepted a line-search step: {:?}",
3964 step,
3965 default_trace
3966 );
3967 assert!(
3968 extended_trace
3969 .iter()
3970 .any(|entry| entry.accepted_step.is_some()),
3971 "step {} extended trace never accepted a line-search step: {:?}",
3972 step,
3973 extended_trace
3974 );
3975 assert!(
3976 default_trace.len() >= 150,
3977 "step {} default trace terminated too early: {:?}",
3978 step,
3979 default_trace
3980 );
3981 assert!(
3982 extended_trace.len() >= default_trace.len(),
3983 "step {} extended trace shorter than default: default={} extended={}",
3984 step,
3985 default_trace.len(),
3986 extended_trace.len()
3987 );
3988 assert!(
3989 last_default_grad > 0.1 && last_extended_grad > 0.1,
3990 "step {} gradients converged unexpectedly: default={} extended={}",
3991 step,
3992 last_default_grad,
3993 last_extended_grad
3994 );
3995
3996 let min_default_cost = default_trace
3997 .iter()
3998 .map(|entry| entry.cost)
3999 .fold(f64::INFINITY, f64::min);
4000 let min_extended_cost = extended_trace
4001 .iter()
4002 .map(|entry| entry.cost)
4003 .fold(f64::INFINITY, f64::min);
4004 assert!(
4005 (default_cost - extended_cost) <= 1e-3,
4006 "step {} extended budget changed cost too much: default={} extended={} gap_default={} gap_extended={}",
4007 step,
4008 default_cost,
4009 extended_cost,
4010 default_gap,
4011 extended_gap
4012 );
4013 assert!(
4014 (min_default_cost - min_extended_cost) <= 1e-3,
4015 "step {} extended budget changed min trace cost too much: default={} extended={}",
4016 step,
4017 min_default_cost,
4018 min_extended_cost
4019 );
4020 }
4021 }
4022
4023 #[test]
4024 fn test_mpc_switch_back_reverse_line_search_scan_characterization() {
4025 let expected_trace =
4026 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
4027 let expected_controls = parse_switch_back_reverse_controls(include_str!(
4028 "testdata/mpc_switch_back_reverse_controls.csv"
4029 ));
4030 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
4031 "testdata/mpc_switch_back_reverse_horizon.csv"
4032 ));
4033 let expected_xbar = parse_switch_back_reverse_states(include_str!(
4034 "testdata/mpc_switch_back_reverse_xbar.csv"
4035 ));
4036
4037 let step_75_state = state_from_vector4(&expected_trace[75].state);
4038 let step_75_scan = first_iteration_projection_candidates(
4039 expected_horizon.get(&75).unwrap().0.as_slice(),
4040 expected_xbar.get(&75).unwrap(),
4041 &step_75_state,
4042 expected_controls.get(&74).unwrap(),
4043 expected_controls.get(&75).unwrap(),
4044 &[0.25, 0.5, 1.0],
4045 )
4046 .1;
4047 let step_75_small = &step_75_scan[0];
4048 let step_75_medium = &step_75_scan[1];
4049 let step_75_large = &step_75_scan[2];
4050
4051 assert!(
4052 step_75_large.projected_cost + 0.05 < step_75_medium.projected_cost,
4053 "step 75 larger initial step did not improve projected cost enough: {:?}",
4054 step_75_scan
4055 );
4056 assert!(
4057 step_75_large.projected_cost + 0.25 < step_75_small.projected_cost,
4058 "step 75 larger initial step did not beat default projected cost enough: {:?}",
4059 step_75_scan
4060 );
4061 assert!(
4062 step_75_large.projected_to_python[1] + 0.05 < step_75_small.projected_to_python[1],
4063 "step 75 larger initial step did not move steering closer to python enough: {:?}",
4064 step_75_scan
4065 );
4066
4067 let step_78_state = state_from_vector4(&expected_trace[78].state);
4068 let step_78_scan = first_iteration_projection_candidates(
4069 expected_horizon.get(&78).unwrap().0.as_slice(),
4070 expected_xbar.get(&78).unwrap(),
4071 &step_78_state,
4072 expected_controls.get(&77).unwrap(),
4073 expected_controls.get(&78).unwrap(),
4074 &[0.25, 0.5, 1.0],
4075 )
4076 .1;
4077 let step_78_small = &step_78_scan[0];
4078 let step_78_medium = &step_78_scan[1];
4079 let step_78_large = &step_78_scan[2];
4080
4081 assert!(
4082 step_78_small.projected_cost + 0.15 < step_78_medium.projected_cost,
4083 "step 78 default initial step should remain better than 0.5: {:?}",
4084 step_78_scan
4085 );
4086 assert!(
4087 step_78_small.projected_cost + 0.2 < step_78_large.projected_cost,
4088 "step 78 default initial step should remain better than 1.0: {:?}",
4089 step_78_scan
4090 );
4091 }
4092
4093 #[test]
4094 fn test_mpc_switch_back_reverse_candidate_expansion_improves_python_style_cost() {
4095 let expected_trace =
4096 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
4097 let expected_controls = parse_switch_back_reverse_controls(include_str!(
4098 "testdata/mpc_switch_back_reverse_controls.csv"
4099 ));
4100 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
4101 "testdata/mpc_switch_back_reverse_horizon.csv"
4102 ));
4103 let expected_xbar = parse_switch_back_reverse_states(include_str!(
4104 "testdata/mpc_switch_back_reverse_xbar.csv"
4105 ));
4106 let step_candidates = [1.0, 0.5, 0.25, 0.125, 0.0625];
4107
4108 for step in [75usize, 78usize] {
4109 let expected_state = &expected_trace[step].state;
4110 let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
4111 let python_controls = expected_controls.get(&step).unwrap();
4112 let (expected_xref_for_step, _) = expected_horizon.get(&step).unwrap();
4113 let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
4114 let state = state_from_vector4(expected_state);
4115
4116 let (default_controls, _) = optimize_linearized_controls_without_angle_wrap_with_trace(
4117 expected_xref_for_step,
4118 expected_xbar_for_step,
4119 &state,
4120 python_warm_start,
4121 QP_MAX_ITERS,
4122 LINE_SEARCH_ITERS,
4123 );
4124 let (expanded_controls, expanded_trace) =
4125 optimize_linearized_controls_without_angle_wrap_with_candidate_expansion(
4126 expected_xref_for_step,
4127 expected_xbar_for_step,
4128 &state,
4129 python_warm_start,
4130 QP_MAX_ITERS,
4131 &step_candidates,
4132 );
4133
4134 let python_x = linearized_rollout_without_angle_wrap(
4135 &state,
4136 expected_xbar_for_step,
4137 python_controls,
4138 );
4139 let python_cost =
4140 compute_cost_without_angle_wrap(&python_x, expected_xref_for_step, python_controls);
4141 let default_x = linearized_rollout_without_angle_wrap(
4142 &state,
4143 expected_xbar_for_step,
4144 &default_controls,
4145 );
4146 let default_cost = compute_cost_without_angle_wrap(
4147 &default_x,
4148 expected_xref_for_step,
4149 &default_controls,
4150 );
4151 let expanded_x = linearized_rollout_without_angle_wrap(
4152 &state,
4153 expected_xbar_for_step,
4154 &expanded_controls,
4155 );
4156 let expanded_cost = compute_cost_without_angle_wrap(
4157 &expanded_x,
4158 expected_xref_for_step,
4159 &expanded_controls,
4160 );
4161 let default_gap = default_cost - python_cost;
4162 let expanded_gap = expanded_cost - python_cost;
4163
4164 assert!(
4165 expanded_trace.iter().any(|entry| {
4166 entry
4167 .accepted_step
4168 .is_some_and(|accepted| (accepted - 0.25).abs() > 1e-12)
4169 }),
4170 "step {} candidate expansion never used a non-default step: {:?}",
4171 step,
4172 expanded_trace
4173 );
4174
4175 match step {
4176 75 => {
4177 assert!(
4178 default_gap > 0.1,
4179 "step 75 default gap regressed unexpectedly: {}",
4180 default_gap
4181 );
4182 assert!(
4183 expanded_gap < 0.0,
4184 "step 75 candidate expansion did not beat python-style reference: {}",
4185 expanded_gap
4186 );
4187 assert!(
4188 expanded_cost + 0.1 < default_cost,
4189 "step 75 candidate expansion did not improve enough: default={} expanded={}",
4190 default_cost,
4191 expanded_cost
4192 );
4193 }
4194 78 => {
4195 assert!(
4196 expanded_cost <= default_cost + 1e-6,
4197 "step 78 candidate expansion regressed: default={} expanded={}",
4198 default_cost,
4199 expanded_cost
4200 );
4201 assert!(
4202 expanded_gap <= default_gap + 1e-6,
4203 "step 78 candidate expansion widened gap: default_gap={} expanded_gap={}",
4204 default_gap,
4205 expanded_gap
4206 );
4207 }
4208 _ => unreachable!(),
4209 }
4210 }
4211 }
4212
4213 #[test]
4214 fn test_mpc_switch_back_reverse_wrapped_candidate_expansion_needs_small_step_fallback() {
4215 let expected_trace =
4216 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
4217 let expected_controls = parse_switch_back_reverse_controls(include_str!(
4218 "testdata/mpc_switch_back_reverse_controls.csv"
4219 ));
4220 let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
4221 "testdata/mpc_switch_back_reverse_horizon.csv"
4222 ));
4223 let expected_xbar = parse_switch_back_reverse_states(include_str!(
4224 "testdata/mpc_switch_back_reverse_xbar.csv"
4225 ));
4226 let step_78_state = state_from_vector4(&expected_trace[78].state);
4227 let python_warm_start = expected_controls.get(&77).unwrap();
4228 let (expected_xref_for_step, _) = expected_horizon.get(&78).unwrap();
4229 let expected_xbar_for_step = expected_xbar.get(&78).unwrap();
4230
4231 let default_controls = optimize_linearized_controls(
4232 expected_xref_for_step,
4233 expected_xbar_for_step,
4234 &step_78_state,
4235 python_warm_start,
4236 );
4237 let (coarse_controls, _coarse_trace) =
4238 optimize_linearized_controls_with_candidate_expansion(
4239 expected_xref_for_step,
4240 expected_xbar_for_step,
4241 &step_78_state,
4242 python_warm_start,
4243 QP_MAX_ITERS,
4244 &[1.0, 0.5, 0.25, 0.125, 0.0625],
4245 );
4246 let (full_controls, _full_trace) = optimize_linearized_controls_with_candidate_expansion(
4247 expected_xref_for_step,
4248 expected_xbar_for_step,
4249 &step_78_state,
4250 python_warm_start,
4251 QP_MAX_ITERS,
4252 &expanded_backtracking_step_candidates(),
4253 );
4254
4255 let (default_x, _, _, _) =
4256 linearized_rollout(&step_78_state, expected_xbar_for_step, &default_controls);
4257 let (coarse_x, _, _, _) =
4258 linearized_rollout(&step_78_state, expected_xbar_for_step, &coarse_controls);
4259 let (full_x, _, _, _) =
4260 linearized_rollout(&step_78_state, expected_xbar_for_step, &full_controls);
4261 let default_cost = compute_cost(&default_x, expected_xref_for_step, &default_controls);
4262 let coarse_cost = compute_cost(&coarse_x, expected_xref_for_step, &coarse_controls);
4263 let full_cost = compute_cost(&full_x, expected_xref_for_step, &full_controls);
4264
4265 assert!(
4269 coarse_cost > default_cost - 1.0,
4270 "step 78 coarse candidate expansion cost unexpectedly much lower: default={} coarse={}",
4271 default_cost,
4272 coarse_cost
4273 );
4274 assert!(
4275 full_cost < default_cost * 6.0,
4276 "step 78 full candidate expansion cost unreasonably high: default={} full={}",
4277 default_cost,
4278 full_cost
4279 );
4280 }
4281
4282 #[test]
4283 fn test_mpc_switch_back_reverse_full_candidate_expansion_preserves_outer_loop_cost() {
4284 for step in [75usize, 78usize] {
4285 let ctx = advance_switch_back_context(step);
4286 let (xref, _) = calc_ref_trajectory(
4287 &ctx.state,
4288 &ctx.cx,
4289 &ctx.cy,
4290 &ctx.cyaw,
4291 &ctx.speed_profile,
4292 UPSTREAM_SWITCH_BACK_TICK,
4293 ctx.target_index,
4294 );
4295
4296 let default_result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
4297 let expanded_result = iterative_linear_mpc_control_with_test_optimizer(
4298 &xref,
4299 &ctx.state,
4300 &ctx.warm_start,
4301 optimize_linearized_controls_with_full_candidate_expansion,
4302 );
4303
4304 let default_cost =
4305 compute_cost(&default_result.predicted, &xref, &default_result.controls);
4306 let expanded_cost =
4307 compute_cost(&expanded_result.predicted, &xref, &expanded_result.controls);
4308
4309 assert!(
4310 expanded_cost <= default_cost + 1e-9,
4311 "step {} full candidate expansion outer-loop cost regressed: default={} expanded={}",
4312 step,
4313 default_cost,
4314 expanded_cost
4315 );
4316 }
4317 }
4318
4319 #[test]
4320 fn test_mpc_switch_back_reverse_step_75_matches_pythonrobotics_reference() {
4321 let ctx = advance_switch_back_context(75);
4322 let (xref, target_index) = calc_ref_trajectory(
4323 &ctx.state,
4324 &ctx.cx,
4325 &ctx.cy,
4326 &ctx.cyaw,
4327 &ctx.speed_profile,
4328 UPSTREAM_SWITCH_BACK_TICK,
4329 ctx.target_index,
4330 );
4331 let result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
4332
4333 assert_eq!(target_index, 35);
4334 let state_diff = trace_state_diff(
4335 &ctx.state.to_vector(),
4336 &Vector4::new(
4337 29.037_582_480_114_636,
4338 3.815_773_924_700_736,
4339 2.555_321_662_179_444_3,
4340 3.092_675_307_077_598_7,
4341 ),
4342 );
4343 assert_diff_bounds(
4344 "reverse_step_75_state",
4345 state_diff,
4346 [1e-2, 0.039, 5e-3, 0.17],
4347 );
4348
4349 let expected_xref = [
4350 [
4351 27.746_027_874_214_72,
4352 1.657_608_713_384_614,
4353 TARGET_SPEED,
4354 2.615_811_171_700_94,
4355 ],
4356 [
4357 27.025_250_293_779_735,
4358 2.066_144_795_139_825_5,
4359 TARGET_SPEED,
4360 2.634_462_746_003_994_7,
4361 ],
4362 [
4363 27.025_250_293_779_735,
4364 2.066_144_795_139_825_5,
4365 TARGET_SPEED,
4366 2.634_462_746_003_994_7,
4367 ],
4368 [
4369 26.232_726_746_496_056,
4370 2.499_757_118_116_159_7,
4371 TARGET_SPEED,
4372 2.646_329_056_016_381,
4373 ],
4374 [
4375 26.232_726_746_496_056,
4376 2.499_757_118_116_159_7,
4377 TARGET_SPEED,
4378 2.646_329_056_016_381,
4379 ],
4380 [
4381 25.376_277_983_039_653,
4382 2.958_190_594_397_198_7,
4383 TARGET_SPEED,
4384 2.653_104_280_632_957_7,
4385 ],
4386 ];
4387 let xref_diff = summarize_vec4_sequence_diff(&xref, &expected_xref);
4388 assert_diff_bounds(
4389 "reverse_step_75_xref",
4390 xref_diff,
4391 [1e-10, 1e-10, 1e-12, 1e-10],
4392 );
4393
4394 let expected_controls = [
4395 [0.999_999_999_725_671_9, 0.562_383_993_404_798_6],
4396 [0.999_999_999_289_914_1, 0.457_664_238_591_364_85],
4397 [0.998_139_695_362_811_2, 0.352_944_483_746_078_1],
4398 [-0.296_638_673_829_085_65, 0.248_224_729_252_603_17],
4399 [-0.868_769_805_574_562_3, 0.143_736_759_161_897_54],
4400 ];
4401 let control_diff = summarize_vec2_sequence_diff(&result.controls, &expected_controls);
4402 assert_diff_bounds("reverse_step_75_control", control_diff, [0.18, 0.36]);
4403
4404 let expected_predicted = [
4405 [
4406 29.037_582_480_114_633,
4407 3.815_773_924_700_736,
4408 2.555_321_662_179_444_3,
4409 3.092_675_307_077_598_7,
4410 ],
4411 [
4412 28.527_129_490_412_527,
4413 3.840_763_866_523_892,
4414 2.755_321_662_124_578_6,
4415 3.207_641_067_142_419,
4416 ],
4417 [
4418 27.976_795_579_388_14,
4419 3.804_349_136_114_007_4,
4420 2.955_321_661_982_561_7,
4421 3.308_522_042_381_720_3,
4422 ],
4423 [
4424 27.392_314_838_387_758,
4425 3.705_780_688_303_695,
4426 3.154_949_601_055_123_7,
4427 3.391_967_200_625_691_3,
4428 ],
4429 [
4430 26.777_902_427_743_353,
4431 3.547_962_330_090_222,
4432 3.095_621_866_289_306_5,
4433 3.454_206_605_778_239_7,
4434 ],
4435 [
4436 26.184_119_418_583_78,
4437 3.355_144_266_828_263_6,
4438 2.921_867_905_174_394_3,
4439 3.489_624_562_748_850_7,
4440 ],
4441 ];
4442 let predicted_diff = summarize_vec4_sequence_diff(&result.predicted, &expected_predicted);
4443 assert_diff_bounds(
4444 "reverse_step_75_predicted",
4445 predicted_diff,
4446 [0.12, 0.42, 0.04, 0.33],
4447 );
4448 }
4449
4450 #[test]
4451 fn test_mpc_switch_back_reverse_step_78_matches_pythonrobotics_reference() {
4452 let ctx = advance_switch_back_context(78);
4453 let (xref, target_index) = calc_ref_trajectory(
4454 &ctx.state,
4455 &ctx.cx,
4456 &ctx.cy,
4457 &ctx.cyaw,
4458 &ctx.speed_profile,
4459 UPSTREAM_SWITCH_BACK_TICK,
4460 ctx.target_index,
4461 );
4462 let result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
4463
4464 assert!(target_index.abs_diff(37) <= 1);
4465 let state_diff = trace_state_diff(
4466 &ctx.state.to_vector(),
4467 &Vector4::new(
4468 27.396_351_837_762_67,
4469 3.690_715_343_715_656,
4470 3.155_321_660_492_834_5,
4471 3.307_411_595_480_136,
4472 ),
4473 );
4474 assert_diff_bounds(
4475 "reverse_step_78_state",
4476 state_diff,
4477 [1e-2, 0.33, 5e-3, 0.042],
4478 );
4479
4480 let expected_xref = [
4481 [
4482 26.232_726_746_496_056,
4483 2.499_757_118_116_159_7,
4484 TARGET_SPEED,
4485 2.646_329_056_016_381,
4486 ],
4487 [
4488 25.376_277_983_039_653,
4489 2.958_190_594_397_198_7,
4490 TARGET_SPEED,
4491 2.653_104_280_632_957_7,
4492 ],
4493 [
4494 25.376_277_983_039_653,
4495 2.958_190_594_397_198_7,
4496 TARGET_SPEED,
4497 2.653_104_280_632_957_7,
4498 ],
4499 [
4500 24.463_724_754_086_51,
4501 3.441_190_136_066_525_4,
4502 TARGET_SPEED,
4503 2.655_851_531_497_457_3,
4504 ],
4505 [
4506 23.502_887_810_312_604,
4507 3.948_500_655_207_722_4,
4508 TARGET_SPEED,
4509 2.655_248_764_939_293_5,
4510 ],
4511 [
4512 23.502_887_810_312_604,
4513 3.948_500_655_207_722_4,
4514 TARGET_SPEED,
4515 2.655_248_764_939_293_5,
4516 ],
4517 ];
4518 let xref_diff = summarize_vec4_sequence_diff(&xref, &expected_xref);
4519 assert_diff_bounds(
4520 "reverse_step_78_xref",
4521 xref_diff,
4522 [1e-10, 1e-10, 1e-12, 1e-10],
4523 );
4524
4525 let expected_controls = [
4526 [0.999_999_999_807_820_6, -0.487_721_747_027_616_45],
4527 [0.782_500_923_395_977_7, -0.477_987_056_920_691],
4528 [-0.698_729_449_948_941_9, -0.456_028_220_361_946_7],
4529 [-0.999_999_995_989_379_2, -0.443_086_192_245_847_8],
4530 [-0.999_999_978_879_091_9, -0.443_921_255_425_959_9],
4531 ];
4532 let control_diff = summarize_vec2_sequence_diff(&result.controls, &expected_controls);
4533 assert_diff_bounds("reverse_step_78_control", control_diff, [1.46, 0.51]);
4534
4535 let expected_predicted = [
4536 [
4537 27.396_351_837_762_67,
4538 3.690_715_343_715_655,
4539 3.155_321_660_492_829_6,
4540 3.307_411_595_480_137_7,
4541 ],
4542 [
4543 26.773_943_492_338_02,
4544 3.586_551_805_024_059,
4545 3.355_321_660_454_389_4,
4546 3.184_298_076_064_967_7,
4547 ],
4548 [
4549 26.103_420_596_888_107,
4550 3.557_898_739_267_037_6,
4551 3.511_821_845_133_581,
4552 3.055_994_050_024_563_7,
4553 ],
4554 [
4555 25.403_376_777_674_16,
4556 3.617_906_440_004_764_7,
4557 3.372_075_955_143_793_5,
4558 2.928_267_659_325_554,
4559 ],
4560 [
4561 24.743_907_999_953_26,
4562 3.760_672_834_366_53,
4563 3.172_075_955_945_920_6,
4564 2.809_186_243_738_390_3,
4565 ],
4566 [
4567 24.143_945_332_102_11,
4568 3.967_706_477_151_346_6,
4569 2.972_075_960_170_106,
4570 2.696_983_141_208_553_6,
4571 ],
4572 ];
4573 let predicted_diff = summarize_vec4_sequence_diff(&result.predicted, &expected_predicted);
4574 assert_diff_bounds(
4575 "reverse_step_78_predicted",
4576 predicted_diff,
4577 [0.26, 0.42, 0.47, 0.48],
4578 );
4579 }
4580
4581 #[test]
4582 #[ignore = "long-running regression scenario"]
4583 fn test_mpc_simulation_reaches_goal_full_course() {
4584 let result = run_mpc_simulation();
4587 let goal_distance = ((result.final_state.x - result.goal.0).powi(2)
4588 + (result.final_state.y - result.goal.1).powi(2))
4589 .sqrt();
4590
4591 assert_eq!(result.target_index, result.final_index);
4592 assert_eq!(result.final_index, 161);
4593 assert!((result.hist_x.len() as isize - 347).abs() <= 8);
4594 assert!(result.reached_goal);
4595 assert!(goal_distance <= GOAL_DIS + 0.25);
4596 assert!(result.hist_x.len() < MAX_SIM_STEPS);
4597 assert_vec4_close(
4598 &result.final_state.to_vector(),
4599 [
4600 -0.014_888_855_512_389,
4601 0.698_023_990_780_443,
4602 0.132_179_560_675_735,
4603 1.589_745_981_585_03,
4604 ],
4605 2e-2,
4606 );
4607 }
4608
4609 #[test]
4610 #[ignore = "long-running regression scenario"]
4611 fn test_mpc_switch_back_full_candidate_expansion_characterizes_closed_loop_tradeoff() {
4612 let original_result = run_switch_back_simulation_with_test_optimizer(
4613 MAX_SIM_STEPS,
4614 optimize_linearized_controls_original,
4615 );
4616 let expanded_result = run_switch_back_simulation_with_test_optimizer(
4617 MAX_SIM_STEPS,
4618 optimize_linearized_controls_with_full_candidate_expansion,
4619 );
4620 let original_goal_distance = ((original_result.final_state.x - original_result.goal.0)
4621 .powi(2)
4622 + (original_result.final_state.y - original_result.goal.1).powi(2))
4623 .sqrt();
4624 let expanded_goal_distance = ((expanded_result.final_state.x - expanded_result.goal.0)
4625 .powi(2)
4626 + (expanded_result.final_state.y - expanded_result.goal.1).powi(2))
4627 .sqrt();
4628
4629 assert!(original_result.reached_goal);
4630 assert!(expanded_result.reached_goal);
4631 assert_eq!(original_result.target_index, original_result.final_index);
4632 assert_eq!(expanded_result.target_index, expanded_result.final_index);
4633 assert_eq!(expanded_result.final_index, 161);
4634 assert_eq!(expanded_result.hist_x.len(), 347);
4635 assert!(expanded_result.hist_x.len() < original_result.hist_x.len());
4636 assert!(expanded_goal_distance <= original_goal_distance + 1e-9);
4637 assert!(expanded_goal_distance <= GOAL_DIS + 0.25);
4638 assert!(expanded_result.hist_x.len() < MAX_SIM_STEPS);
4639 assert_vec4_close(
4640 &expanded_result.final_state.to_vector(),
4641 [
4642 -0.014_888_855_512_389,
4643 0.698_023_990_780_443,
4644 0.132_179_560_675_735,
4645 1.589_745_981_585_03,
4646 ],
4647 5e-3,
4648 );
4649 }
4650
4651 #[test]
4652 #[ignore = "long-running regression scenario"]
4653 fn test_mpc_switch_back_full_candidate_expansion_characterizes_recovery_window() {
4654 let checkpoints = [80usize, 120, 160, 200, 260, 320, 346];
4655 let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4656 MAX_SIM_STEPS,
4657 &checkpoints,
4658 optimize_linearized_controls_original,
4659 );
4660 let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4661 MAX_SIM_STEPS,
4662 &checkpoints,
4663 optimize_linearized_controls_with_full_candidate_expansion,
4664 );
4665
4666 assert_eq!(default_checkpoints.len(), checkpoints.len());
4667 assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4668
4669 for ((default, expanded), expected_step) in default_checkpoints
4670 .iter()
4671 .zip(expanded_checkpoints.iter())
4672 .zip(checkpoints)
4673 {
4674 assert_eq!(default.step, expected_step);
4675 assert_eq!(expanded.step, expected_step);
4676 }
4677
4678 let step_80_delta = goal_distance_delta(&default_checkpoints[0], &expanded_checkpoints[0]);
4679 let step_120_delta = goal_distance_delta(&default_checkpoints[1], &expanded_checkpoints[1]);
4680 let step_160_delta = goal_distance_delta(&default_checkpoints[2], &expanded_checkpoints[2]);
4681 let step_200_delta = goal_distance_delta(&default_checkpoints[3], &expanded_checkpoints[3]);
4682 let step_260_delta = goal_distance_delta(&default_checkpoints[4], &expanded_checkpoints[4]);
4683 let step_320_delta = goal_distance_delta(&default_checkpoints[5], &expanded_checkpoints[5]);
4684 let step_346_delta = goal_distance_delta(&default_checkpoints[6], &expanded_checkpoints[6]);
4685
4686 assert!(
4687 (-0.03..=-0.005).contains(&step_80_delta),
4688 "step 80 delta moved unexpectedly: default={:?} expanded={:?}",
4689 default_checkpoints[0],
4690 expanded_checkpoints[0]
4691 );
4692 assert!(
4693 (0.15..=0.25).contains(&step_120_delta),
4694 "step 120 delta moved unexpectedly: default={:?} expanded={:?}",
4695 default_checkpoints[1],
4696 expanded_checkpoints[1]
4697 );
4698 assert!(
4699 (-0.35..=-0.2).contains(&step_160_delta),
4700 "step 160 delta moved unexpectedly: default={:?} expanded={:?}",
4701 default_checkpoints[2],
4702 expanded_checkpoints[2]
4703 );
4704 assert!(
4705 (-0.35..=-0.15).contains(&step_200_delta),
4706 "step 200 delta moved unexpectedly: default={:?} expanded={:?}",
4707 default_checkpoints[3],
4708 expanded_checkpoints[3]
4709 );
4710 assert!(
4711 (0.8..=1.2).contains(&step_260_delta),
4712 "step 260 delta moved unexpectedly: default={:?} expanded={:?}",
4713 default_checkpoints[4],
4714 expanded_checkpoints[4]
4715 );
4716 assert!(
4717 (1.4..=1.8).contains(&step_320_delta),
4718 "step 320 delta moved unexpectedly: default={:?} expanded={:?}",
4719 default_checkpoints[5],
4720 expanded_checkpoints[5]
4721 );
4722 assert!(
4723 (0.15..=0.25).contains(&step_346_delta),
4724 "step 346 delta moved unexpectedly: default={:?} expanded={:?}",
4725 default_checkpoints[6],
4726 expanded_checkpoints[6]
4727 );
4728
4729 assert_eq!(default_checkpoints[5].target_index, 159);
4730 assert_eq!(expanded_checkpoints[5].target_index, 161);
4731 assert_eq!(default_checkpoints[6].target_index, 161);
4732 assert_eq!(expanded_checkpoints[6].target_index, 161);
4733 }
4734
4735 #[test]
4736 #[ignore = "long-running regression scenario"]
4737 fn test_mpc_switch_back_full_candidate_expansion_characterizes_dense_recovery_window() {
4738 let checkpoints = [
4739 120usize, 130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260,
4740 ];
4741 let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4742 MAX_SIM_STEPS,
4743 &checkpoints,
4744 optimize_linearized_controls_original,
4745 );
4746 let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4747 MAX_SIM_STEPS,
4748 &checkpoints,
4749 optimize_linearized_controls_with_full_candidate_expansion,
4750 );
4751
4752 assert_eq!(default_checkpoints.len(), checkpoints.len());
4753 assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4754
4755 let step_130_default = checkpoint_at_step(&default_checkpoints, 130);
4756 let step_130_expanded = checkpoint_at_step(&expanded_checkpoints, 130);
4757 let step_140_default = checkpoint_at_step(&default_checkpoints, 140);
4758 let step_140_expanded = checkpoint_at_step(&expanded_checkpoints, 140);
4759 let step_180_default = checkpoint_at_step(&default_checkpoints, 180);
4760 let step_180_expanded = checkpoint_at_step(&expanded_checkpoints, 180);
4761 let step_200_default = checkpoint_at_step(&default_checkpoints, 200);
4762 let step_200_expanded = checkpoint_at_step(&expanded_checkpoints, 200);
4763 let step_210_default = checkpoint_at_step(&default_checkpoints, 210);
4764 let step_210_expanded = checkpoint_at_step(&expanded_checkpoints, 210);
4765 let step_220_default = checkpoint_at_step(&default_checkpoints, 220);
4766 let step_220_expanded = checkpoint_at_step(&expanded_checkpoints, 220);
4767 let step_240_default = checkpoint_at_step(&default_checkpoints, 240);
4768 let step_240_expanded = checkpoint_at_step(&expanded_checkpoints, 240);
4769 let step_260_default = checkpoint_at_step(&default_checkpoints, 260);
4770 let step_260_expanded = checkpoint_at_step(&expanded_checkpoints, 260);
4771
4772 let step_130_delta = goal_distance_delta(step_130_default, step_130_expanded);
4773 let step_140_delta = goal_distance_delta(step_140_default, step_140_expanded);
4774 let step_180_delta = goal_distance_delta(step_180_default, step_180_expanded);
4775 let step_200_delta = goal_distance_delta(step_200_default, step_200_expanded);
4776 let step_210_delta = goal_distance_delta(step_210_default, step_210_expanded);
4777 let step_220_delta = goal_distance_delta(step_220_default, step_220_expanded);
4778 let step_240_delta = goal_distance_delta(step_240_default, step_240_expanded);
4779 let step_260_delta = goal_distance_delta(step_260_default, step_260_expanded);
4780
4781 assert!(
4782 (0.1..=0.18).contains(&step_130_delta),
4783 "step 130 delta moved unexpectedly: default={:?} expanded={:?}",
4784 step_130_default,
4785 step_130_expanded
4786 );
4787 assert!(
4788 (-0.2..=-0.05).contains(&step_140_delta),
4789 "step 140 delta moved unexpectedly: default={:?} expanded={:?}",
4790 step_140_default,
4791 step_140_expanded
4792 );
4793 assert!(
4794 (0.05..=0.15).contains(&step_180_delta),
4795 "step 180 delta moved unexpectedly: default={:?} expanded={:?}",
4796 step_180_default,
4797 step_180_expanded
4798 );
4799 assert!(
4800 (-0.35..=-0.15).contains(&step_200_delta),
4801 "step 200 delta moved unexpectedly: default={:?} expanded={:?}",
4802 step_200_default,
4803 step_200_expanded
4804 );
4805 assert!(
4806 (-0.2..=-0.08).contains(&step_210_delta),
4807 "step 210 delta moved unexpectedly: default={:?} expanded={:?}",
4808 step_210_default,
4809 step_210_expanded
4810 );
4811 assert!(
4812 (0.35..=0.55).contains(&step_220_delta),
4813 "step 220 delta moved unexpectedly: default={:?} expanded={:?}",
4814 step_220_default,
4815 step_220_expanded
4816 );
4817 assert!(
4818 (0.3..=0.55).contains(&step_240_delta),
4819 "step 240 delta moved unexpectedly: default={:?} expanded={:?}",
4820 step_240_default,
4821 step_240_expanded
4822 );
4823 assert!(
4824 (0.85..=1.05).contains(&step_260_delta),
4825 "step 260 delta moved unexpectedly: default={:?} expanded={:?}",
4826 step_260_default,
4827 step_260_expanded
4828 );
4829
4830 assert_eq!(step_140_default.target_index, 71);
4831 assert_eq!(step_140_expanded.target_index, 72);
4832 assert_eq!(step_180_default.target_index, 94);
4833 assert_eq!(step_180_expanded.target_index, 95);
4834 assert_eq!(step_220_default.target_index, 105);
4835 assert_eq!(step_220_expanded.target_index, 106);
4836 assert_eq!(step_240_default.target_index, 115);
4837 assert_eq!(step_240_expanded.target_index, 117);
4838 assert_eq!(step_260_default.target_index, 127);
4839 assert_eq!(step_260_expanded.target_index, 129);
4840 }
4841
4842 #[test]
4843 #[ignore = "long-running regression scenario"]
4844 fn test_mpc_switch_back_full_candidate_expansion_characterizes_flip_window_210_220() {
4845 let checkpoints = [210usize, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220];
4846 let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4847 MAX_SIM_STEPS,
4848 &checkpoints,
4849 optimize_linearized_controls_original,
4850 );
4851 let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4852 MAX_SIM_STEPS,
4853 &checkpoints,
4854 optimize_linearized_controls_with_full_candidate_expansion,
4855 );
4856
4857 assert_eq!(default_checkpoints.len(), checkpoints.len());
4858 assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4859
4860 for ((default, expanded), expected_step) in default_checkpoints
4861 .iter()
4862 .zip(expanded_checkpoints.iter())
4863 .zip(checkpoints)
4864 {
4865 assert_eq!(default.step, expected_step);
4866 assert_eq!(expanded.step, expected_step);
4867 }
4868
4869 let step_210_default = checkpoint_at_step(&default_checkpoints, 210);
4870 let step_210_expanded = checkpoint_at_step(&expanded_checkpoints, 210);
4871 let step_212_default = checkpoint_at_step(&default_checkpoints, 212);
4872 let step_212_expanded = checkpoint_at_step(&expanded_checkpoints, 212);
4873 let step_213_default = checkpoint_at_step(&default_checkpoints, 213);
4874 let step_213_expanded = checkpoint_at_step(&expanded_checkpoints, 213);
4875 let step_214_default = checkpoint_at_step(&default_checkpoints, 214);
4876 let step_214_expanded = checkpoint_at_step(&expanded_checkpoints, 214);
4877 let step_216_default = checkpoint_at_step(&default_checkpoints, 216);
4878 let step_216_expanded = checkpoint_at_step(&expanded_checkpoints, 216);
4879 let step_220_default = checkpoint_at_step(&default_checkpoints, 220);
4880 let step_220_expanded = checkpoint_at_step(&expanded_checkpoints, 220);
4881
4882 let step_210_delta = goal_distance_delta(step_210_default, step_210_expanded);
4883 let step_212_delta = goal_distance_delta(step_212_default, step_212_expanded);
4884 let step_213_delta = goal_distance_delta(step_213_default, step_213_expanded);
4885 let step_214_delta = goal_distance_delta(step_214_default, step_214_expanded);
4886 let step_216_delta = goal_distance_delta(step_216_default, step_216_expanded);
4887 let step_220_delta = goal_distance_delta(step_220_default, step_220_expanded);
4888
4889 assert!(
4890 (-0.18..=-0.1).contains(&step_210_delta),
4891 "step 210 delta moved unexpectedly: default={:?} expanded={:?}",
4892 step_210_default,
4893 step_210_expanded
4894 );
4895 assert!(
4896 (-0.08..=-0.03).contains(&step_212_delta),
4897 "step 212 delta moved unexpectedly: default={:?} expanded={:?}",
4898 step_212_default,
4899 step_212_expanded
4900 );
4901 assert!(
4902 (0.0..=0.02).contains(&step_213_delta),
4903 "step 213 delta moved unexpectedly: default={:?} expanded={:?}",
4904 step_213_default,
4905 step_213_expanded
4906 );
4907 assert!(
4908 (0.05..=0.09).contains(&step_214_delta),
4909 "step 214 delta moved unexpectedly: default={:?} expanded={:?}",
4910 step_214_default,
4911 step_214_expanded
4912 );
4913 assert!(
4914 (0.18..=0.25).contains(&step_216_delta),
4915 "step 216 delta moved unexpectedly: default={:?} expanded={:?}",
4916 step_216_default,
4917 step_216_expanded
4918 );
4919 assert!(
4920 (0.42..=0.5).contains(&step_220_delta),
4921 "step 220 delta moved unexpectedly: default={:?} expanded={:?}",
4922 step_220_default,
4923 step_220_expanded
4924 );
4925
4926 assert_eq!(step_210_default.target_index, 103);
4927 assert_eq!(step_210_expanded.target_index, 103);
4928 assert_eq!(step_212_default.target_index, 103);
4929 assert_eq!(step_212_expanded.target_index, 103);
4930 assert_eq!(step_213_default.target_index, 103);
4931 assert_eq!(step_213_expanded.target_index, 103);
4932 assert_eq!(step_214_default.target_index, 103);
4933 assert_eq!(step_214_expanded.target_index, 104);
4934 assert_eq!(step_216_default.target_index, 103);
4935 assert_eq!(step_216_expanded.target_index, 104);
4936 assert_eq!(step_220_default.target_index, 105);
4937 assert_eq!(step_220_expanded.target_index, 106);
4938 }
4939
4940 #[test]
4941 #[ignore = "long-running regression scenario"]
4942 fn test_mpc_switch_back_full_candidate_expansion_characterizes_progression_to_recovery() {
4943 let checkpoints = [213usize, 214, 220, 230, 240, 260];
4944 let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4945 MAX_SIM_STEPS,
4946 &checkpoints,
4947 optimize_linearized_controls_original,
4948 );
4949 let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4950 MAX_SIM_STEPS,
4951 &checkpoints,
4952 optimize_linearized_controls_with_full_candidate_expansion,
4953 );
4954
4955 assert_eq!(default_checkpoints.len(), checkpoints.len());
4956 assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4957
4958 let step_213_default = checkpoint_at_step(&default_checkpoints, 213);
4959 let step_213_expanded = checkpoint_at_step(&expanded_checkpoints, 213);
4960 let step_214_default = checkpoint_at_step(&default_checkpoints, 214);
4961 let step_214_expanded = checkpoint_at_step(&expanded_checkpoints, 214);
4962 let step_220_default = checkpoint_at_step(&default_checkpoints, 220);
4963 let step_220_expanded = checkpoint_at_step(&expanded_checkpoints, 220);
4964 let step_230_default = checkpoint_at_step(&default_checkpoints, 230);
4965 let step_230_expanded = checkpoint_at_step(&expanded_checkpoints, 230);
4966 let step_240_default = checkpoint_at_step(&default_checkpoints, 240);
4967 let step_240_expanded = checkpoint_at_step(&expanded_checkpoints, 240);
4968 let step_260_default = checkpoint_at_step(&default_checkpoints, 260);
4969 let step_260_expanded = checkpoint_at_step(&expanded_checkpoints, 260);
4970
4971 let step_214_y_delta = step_214_default.state[1] - step_214_expanded.state[1];
4972 let step_214_speed_delta = step_214_default.state[2] - step_214_expanded.state[2];
4973 let step_214_yaw_delta = angle_diff(step_214_default.state[3], step_214_expanded.state[3]);
4974 let step_220_y_delta = step_220_default.state[1] - step_220_expanded.state[1];
4975 let step_220_speed_delta = step_220_default.state[2] - step_220_expanded.state[2];
4976 let step_220_yaw_delta = angle_diff(step_220_default.state[3], step_220_expanded.state[3]);
4977 let step_230_y_delta = step_230_default.state[1] - step_230_expanded.state[1];
4978 let step_230_speed_delta = step_230_default.state[2] - step_230_expanded.state[2];
4979 let step_230_yaw_delta = angle_diff(step_230_default.state[3], step_230_expanded.state[3]);
4980 let step_240_y_delta = step_240_default.state[1] - step_240_expanded.state[1];
4981 let step_240_speed_delta = step_240_default.state[2] - step_240_expanded.state[2];
4982 let step_240_yaw_delta = angle_diff(step_240_default.state[3], step_240_expanded.state[3]);
4983 let step_260_y_delta = step_260_default.state[1] - step_260_expanded.state[1];
4984 let step_260_yaw_delta = angle_diff(step_260_default.state[3], step_260_expanded.state[3]);
4985
4986 assert!(
4987 (0.0..=0.02).contains(&goal_distance_delta(step_213_default, step_213_expanded)),
4988 "step 213 delta moved unexpectedly: default={:?} expanded={:?}",
4989 step_213_default,
4990 step_213_expanded
4991 );
4992 assert!(
4993 (0.05..=0.09).contains(&goal_distance_delta(step_214_default, step_214_expanded)),
4994 "step 214 delta moved unexpectedly: default={:?} expanded={:?}",
4995 step_214_default,
4996 step_214_expanded
4997 );
4998 assert!(
4999 (0.42..=0.5).contains(&goal_distance_delta(step_220_default, step_220_expanded)),
5000 "step 220 delta moved unexpectedly: default={:?} expanded={:?}",
5001 step_220_default,
5002 step_220_expanded
5003 );
5004 assert!(
5005 (0.5..=0.65).contains(&goal_distance_delta(step_230_default, step_230_expanded)),
5006 "step 230 delta moved unexpectedly: default={:?} expanded={:?}",
5007 step_230_default,
5008 step_230_expanded
5009 );
5010 assert!(
5011 (0.35..=0.5).contains(&goal_distance_delta(step_240_default, step_240_expanded)),
5012 "step 240 delta moved unexpectedly: default={:?} expanded={:?}",
5013 step_240_default,
5014 step_240_expanded
5015 );
5016 assert!(
5017 (0.9..=1.05).contains(&goal_distance_delta(step_260_default, step_260_expanded)),
5018 "step 260 delta moved unexpectedly: default={:?} expanded={:?}",
5019 step_260_default,
5020 step_260_expanded
5021 );
5022
5023 assert_eq!(step_213_default.target_index, 103);
5024 assert_eq!(step_213_expanded.target_index, 103);
5025 assert_eq!(step_214_default.target_index, 103);
5026 assert_eq!(step_214_expanded.target_index, 104);
5027 assert_eq!(step_220_default.target_index, 105);
5028 assert_eq!(step_220_expanded.target_index, 106);
5029 assert_eq!(step_230_default.target_index, 109);
5030 assert_eq!(step_230_expanded.target_index, 111);
5031 assert_eq!(step_240_default.target_index, 115);
5032 assert_eq!(step_240_expanded.target_index, 117);
5033 assert_eq!(step_260_default.target_index, 127);
5034 assert_eq!(step_260_expanded.target_index, 129);
5035
5036 assert!(
5037 (-1.1..=-0.95).contains(&step_214_y_delta),
5038 "step 214 y delta moved unexpectedly: default={:?} expanded={:?}",
5039 step_214_default,
5040 step_214_expanded
5041 );
5042 assert!((0.25..=0.32).contains(&step_214_speed_delta));
5043 assert!((-0.22..=-0.12).contains(&step_214_yaw_delta));
5044 assert!(
5045 (-0.95..=-0.75).contains(&step_220_y_delta),
5046 "step 220 y delta moved unexpectedly: default={:?} expanded={:?}",
5047 step_220_default,
5048 step_220_expanded
5049 );
5050 assert!((0.12..=0.22).contains(&step_220_speed_delta));
5051 assert!((-0.12..=-0.03).contains(&step_220_yaw_delta));
5052 assert!(
5053 (-1.2..=-0.95).contains(&step_230_y_delta),
5054 "step 230 y delta moved unexpectedly: default={:?} expanded={:?}",
5055 step_230_default,
5056 step_230_expanded
5057 );
5058 assert!((0.05..=0.15).contains(&step_230_speed_delta));
5059 assert!((-0.05..=0.01).contains(&step_230_yaw_delta));
5060 assert!(
5061 (-1.0..=-0.75).contains(&step_240_y_delta),
5062 "step 240 y delta moved unexpectedly: default={:?} expanded={:?}",
5063 step_240_default,
5064 step_240_expanded
5065 );
5066 assert!((-0.06..=0.02).contains(&step_240_speed_delta));
5067 assert!((-0.09..=-0.03).contains(&step_240_yaw_delta));
5068 assert!(
5069 (0.3..=0.6).contains(&step_260_y_delta),
5070 "step 260 y delta moved unexpectedly: default={:?} expanded={:?}",
5071 step_260_default,
5072 step_260_expanded
5073 );
5074 assert!(
5075 (-0.27..=-0.15).contains(&step_260_yaw_delta),
5076 "step 260 yaw delta moved unexpectedly: default={:?} expanded={:?}",
5077 step_260_default,
5078 step_260_expanded
5079 );
5080 }
5081
5082 #[test]
5083 #[ignore = "long-running regression scenario"]
5084 fn test_mpc_switch_back_full_candidate_expansion_characterizes_late_window_to_finish() {
5085 let checkpoints = [260usize, 280, 300, 320, 340, 346];
5086 let terminal_checkpoints: Vec<_> = (320usize..=350).collect();
5087 let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
5088 MAX_SIM_STEPS,
5089 &checkpoints,
5090 optimize_linearized_controls_original,
5091 );
5092 let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
5093 MAX_SIM_STEPS,
5094 &checkpoints,
5095 optimize_linearized_controls_with_full_candidate_expansion,
5096 );
5097 let default_terminal = collect_switch_back_goal_condition_snapshots_with_test_optimizer(
5098 MAX_SIM_STEPS,
5099 &terminal_checkpoints,
5100 optimize_linearized_controls_original,
5101 );
5102 let expanded_terminal = collect_switch_back_goal_condition_snapshots_with_test_optimizer(
5103 MAX_SIM_STEPS,
5104 &terminal_checkpoints,
5105 optimize_linearized_controls_with_full_candidate_expansion,
5106 );
5107 let default_result = run_switch_back_simulation_with_test_optimizer(
5108 MAX_SIM_STEPS,
5109 optimize_linearized_controls_original,
5110 );
5111 let expanded_result = run_switch_back_simulation_with_test_optimizer(
5112 MAX_SIM_STEPS,
5113 optimize_linearized_controls_with_full_candidate_expansion,
5114 );
5115
5116 let step_260_default = checkpoint_at_step(&default_checkpoints, 260);
5117 let step_260_expanded = checkpoint_at_step(&expanded_checkpoints, 260);
5118 let step_280_default = checkpoint_at_step(&default_checkpoints, 280);
5119 let step_280_expanded = checkpoint_at_step(&expanded_checkpoints, 280);
5120 let step_300_default = checkpoint_at_step(&default_checkpoints, 300);
5121 let step_300_expanded = checkpoint_at_step(&expanded_checkpoints, 300);
5122 let step_320_default = checkpoint_at_step(&default_checkpoints, 320);
5123 let step_320_expanded = checkpoint_at_step(&expanded_checkpoints, 320);
5124 let step_340_default = checkpoint_at_step(&default_checkpoints, 340);
5125 let step_340_expanded = checkpoint_at_step(&expanded_checkpoints, 340);
5126 let step_346_default = checkpoint_at_step(&default_checkpoints, 346);
5127 let step_346_expanded = checkpoint_at_step(&expanded_checkpoints, 346);
5128 let (cx, cy, cyaw) = generate_switch_back_course();
5129
5130 let slope_260_280_default = goal_distance_slope(step_260_default, step_280_default);
5131 let slope_260_280_expanded = goal_distance_slope(step_260_expanded, step_280_expanded);
5132 let slope_280_300_default = goal_distance_slope(step_280_default, step_300_default);
5133 let slope_280_300_expanded = goal_distance_slope(step_280_expanded, step_300_expanded);
5134 let slope_300_320_default = goal_distance_slope(step_300_default, step_320_default);
5135 let slope_300_320_expanded = goal_distance_slope(step_300_expanded, step_320_expanded);
5136 let slope_320_340_default = goal_distance_slope(step_320_default, step_340_default);
5137 let slope_320_340_expanded = goal_distance_slope(step_320_expanded, step_340_expanded);
5138 let slope_340_346_default = goal_distance_slope(step_340_default, step_346_default);
5139 let slope_340_346_expanded = goal_distance_slope(step_340_expanded, step_346_expanded);
5140 let lat_abs_slope_300_320_default = scalar_slope(
5141 checkpoint_lateral_error(step_300_default, &cx, &cy, &cyaw).abs(),
5142 checkpoint_lateral_error(step_320_default, &cx, &cy, &cyaw).abs(),
5143 20,
5144 );
5145 let lat_abs_slope_300_320_expanded = scalar_slope(
5146 checkpoint_lateral_error(step_300_expanded, &cx, &cy, &cyaw).abs(),
5147 checkpoint_lateral_error(step_320_expanded, &cx, &cy, &cyaw).abs(),
5148 20,
5149 );
5150 let heading_abs_slope_300_320_default = scalar_slope(
5151 checkpoint_heading_error(step_300_default, &cyaw).abs(),
5152 checkpoint_heading_error(step_320_default, &cyaw).abs(),
5153 20,
5154 );
5155 let heading_abs_slope_300_320_expanded = scalar_slope(
5156 checkpoint_heading_error(step_300_expanded, &cyaw).abs(),
5157 checkpoint_heading_error(step_320_expanded, &cyaw).abs(),
5158 20,
5159 );
5160 let speed_slope_300_320_default =
5161 scalar_slope(step_300_default.state[2], step_320_default.state[2], 20);
5162 let speed_slope_300_320_expanded =
5163 scalar_slope(step_300_expanded.state[2], step_320_expanded.state[2], 20);
5164 let lat_abs_slope_320_340_default = scalar_slope(
5165 checkpoint_lateral_error(step_320_default, &cx, &cy, &cyaw).abs(),
5166 checkpoint_lateral_error(step_340_default, &cx, &cy, &cyaw).abs(),
5167 20,
5168 );
5169 let lat_abs_slope_320_340_expanded = scalar_slope(
5170 checkpoint_lateral_error(step_320_expanded, &cx, &cy, &cyaw).abs(),
5171 checkpoint_lateral_error(step_340_expanded, &cx, &cy, &cyaw).abs(),
5172 20,
5173 );
5174 let heading_abs_slope_320_340_default = scalar_slope(
5175 checkpoint_heading_error(step_320_default, &cyaw).abs(),
5176 checkpoint_heading_error(step_340_default, &cyaw).abs(),
5177 20,
5178 );
5179 let heading_abs_slope_320_340_expanded = scalar_slope(
5180 checkpoint_heading_error(step_320_expanded, &cyaw).abs(),
5181 checkpoint_heading_error(step_340_expanded, &cyaw).abs(),
5182 20,
5183 );
5184 let speed_slope_320_340_default =
5185 scalar_slope(step_320_default.state[2], step_340_default.state[2], 20);
5186 let speed_slope_320_340_expanded =
5187 scalar_slope(step_320_expanded.state[2], step_340_expanded.state[2], 20);
5188
5189 assert_eq!(
5190 target_index_increment(step_260_default, step_280_default),
5191 10
5192 );
5193 assert_eq!(
5194 target_index_increment(step_260_expanded, step_280_expanded),
5195 10
5196 );
5197 assert!(slope_260_280_expanded > slope_260_280_default);
5198
5199 assert_eq!(
5200 target_index_increment(step_280_default, step_300_default),
5201 10
5202 );
5203 assert_eq!(
5204 target_index_increment(step_280_expanded, step_300_expanded),
5205 11
5206 );
5207 assert!(slope_280_300_expanded > slope_280_300_default);
5208
5209 assert_eq!(
5210 target_index_increment(step_300_default, step_320_default),
5211 12
5212 );
5213 assert_eq!(
5214 target_index_increment(step_300_expanded, step_320_expanded),
5215 11
5216 );
5217 assert!(slope_300_320_default > slope_300_320_expanded);
5218 assert!(lat_abs_slope_300_320_default < lat_abs_slope_300_320_expanded - 2.0e-4);
5219 assert!(heading_abs_slope_300_320_default < heading_abs_slope_300_320_expanded - 2.0e-4);
5220 assert!(speed_slope_300_320_expanded > speed_slope_300_320_default + 0.05);
5221
5222 assert_eq!(step_320_default.target_index, 159);
5223 assert_eq!(step_320_expanded.target_index, 161);
5224 assert_eq!(
5225 target_index_increment(step_320_default, step_340_default),
5226 2
5227 );
5228 assert_eq!(
5229 target_index_increment(step_320_expanded, step_340_expanded),
5230 0
5231 );
5232 assert!(slope_320_340_default > 0.0);
5233 assert!(slope_320_340_expanded < 0.0);
5234 assert!(lat_abs_slope_320_340_expanded < lat_abs_slope_320_340_default - 5.0e-5);
5235 assert!(heading_abs_slope_320_340_default < heading_abs_slope_320_340_expanded - 1.0e-4);
5236 assert!(speed_slope_320_340_default > speed_slope_320_340_expanded + 0.04);
5237
5238 assert_eq!(step_340_default.target_index, 161);
5239 assert_eq!(step_340_expanded.target_index, 161);
5240 assert_eq!(
5241 target_index_increment(step_340_default, step_346_default),
5242 0
5243 );
5244 assert_eq!(
5245 target_index_increment(step_340_expanded, step_346_expanded),
5246 0
5247 );
5248 assert!(slope_340_346_default > slope_340_346_expanded);
5249 assert!(step_340_default.goal_distance > step_340_expanded.goal_distance + 0.4);
5250 assert!(step_340_default.state[2] > step_340_expanded.state[2] + 0.3);
5251 assert!(
5252 (0.15..=0.25).contains(&goal_distance_delta(step_346_default, step_346_expanded)),
5253 "step 346 delta moved unexpectedly: default={:?} expanded={:?}",
5254 step_346_default,
5255 step_346_expanded
5256 );
5257
5258 assert_eq!(default_result.hist_x.len() - 1, 350);
5259 assert_eq!(expanded_result.hist_x.len() - 1, 346);
5260
5261 assert_eq!(
5262 first_goal_condition_step(&default_terminal, |s| s.near_end),
5263 Some(320)
5264 );
5265 assert_eq!(
5266 first_goal_condition_step(&default_terminal, |s| s.near_goal),
5267 Some(321)
5268 );
5269 assert_eq!(
5270 first_goal_condition_step(&default_terminal, |s| s.stopped),
5271 Some(333)
5272 );
5273 assert_eq!(
5274 first_goal_condition_step(&default_terminal, |s| s.reached_goal),
5275 Some(350)
5276 );
5277
5278 assert_eq!(
5279 first_goal_condition_step(&expanded_terminal, |s| s.near_end),
5280 Some(320)
5281 );
5282 assert_eq!(
5283 first_goal_condition_step(&expanded_terminal, |s| s.near_goal),
5284 Some(320)
5285 );
5286 assert_eq!(
5287 first_goal_condition_step(&expanded_terminal, |s| s.stopped),
5288 Some(329)
5289 );
5290 assert_eq!(
5291 first_goal_condition_step(&expanded_terminal, |s| s.reached_goal),
5292 Some(346)
5293 );
5294
5295 let default_step_346 = goal_snapshot_at_step(&default_terminal, 346);
5296 let default_step_350 = goal_snapshot_at_step(&default_terminal, 350);
5297 let expanded_step_329 = goal_snapshot_at_step(&expanded_terminal, 329);
5298 let expanded_step_330 = goal_snapshot_at_step(&expanded_terminal, 330);
5299 let expanded_step_335 = goal_snapshot_at_step(&expanded_terminal, 335);
5300 let expanded_step_346 = goal_snapshot_at_step(&expanded_terminal, 346);
5301 let expanded_first_positive_speed =
5302 first_goal_condition_step(&expanded_terminal, |s| s.speed > 0.0);
5303 let expanded_reacquired_near_goal = expanded_terminal
5304 .iter()
5305 .skip_while(|snapshot| snapshot.near_goal)
5306 .find(|snapshot| snapshot.near_goal)
5307 .map(|snapshot| snapshot.step);
5308 let expanded_course_curvature =
5309 course_curvature_at_index(&cx, &cy, &cyaw, expanded_step_330.target_index);
5310
5311 assert!(default_step_346.near_goal);
5312 assert!(default_step_346.near_end);
5313 assert!(!default_step_346.stopped);
5314 assert!(!default_step_346.reached_goal);
5315
5316 assert!(!expanded_step_329.near_goal);
5317 assert!(expanded_step_329.near_end);
5318 assert!(expanded_step_329.stopped);
5319 assert!(expanded_step_329.speed < 0.0);
5320 assert!(expanded_step_329.motion_curvature > 0.0);
5321
5322 assert!(!expanded_step_330.near_goal);
5323 assert!(expanded_step_330.near_end);
5324 assert!(!expanded_step_330.stopped);
5325 assert!(!expanded_step_330.reached_goal);
5326 assert_eq!(expanded_first_positive_speed, Some(330));
5327 assert_eq!(expanded_reacquired_near_goal, Some(335));
5328 assert!(expanded_step_330.speed > 0.0);
5329 assert!(expanded_step_330.motion_curvature < 0.0);
5330 assert!(expanded_course_curvature > 0.0);
5331
5332 assert!(expanded_step_335.near_goal);
5333 assert!(expanded_step_335.near_end);
5334 assert!(!expanded_step_335.stopped);
5335 assert!(!expanded_step_335.reached_goal);
5336 assert!(expanded_step_335.goal_distance < GOAL_DIS);
5337 assert!(expanded_step_335.heading_error.abs() < expanded_step_330.heading_error.abs());
5338 assert!(expanded_step_335.motion_curvature < 0.0);
5339 assert!(expanded_step_335.goal_distance < expanded_step_330.goal_distance);
5340 assert!(expanded_step_335.speed > expanded_step_330.speed);
5341
5342 assert!(expanded_step_346.near_goal);
5343 assert!(expanded_step_346.near_end);
5344 assert!(expanded_step_346.stopped);
5345 assert!(expanded_step_346.reached_goal);
5346
5347 assert!(default_step_350.near_goal);
5348 assert!(default_step_350.near_end);
5349 assert!(default_step_350.stopped);
5350 assert!(default_step_350.reached_goal);
5351 }
5352
5353 #[test]
5354 #[ignore = "long-running regression scenario"]
5355 fn test_mpc_switch_back_full_candidate_expansion_characterizes_terminal_reentry_controls() {
5356 let checkpoints: Vec<_> = (329usize..=350).collect();
5357 let default_snapshots = collect_switch_back_goal_control_snapshots_with_test_optimizer(
5358 MAX_SIM_STEPS,
5359 &checkpoints,
5360 optimize_linearized_controls_original,
5361 );
5362 let expanded_snapshots = collect_switch_back_goal_control_snapshots_with_test_optimizer(
5363 MAX_SIM_STEPS,
5364 &checkpoints,
5365 optimize_linearized_controls_with_full_candidate_expansion,
5366 );
5367
5368 let default_first_stopped =
5369 first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.stopped);
5370 let expanded_first_stopped =
5371 first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.stopped);
5372 let default_first_positive_speed =
5373 first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.speed > 0.0);
5374 let expanded_first_positive_speed =
5375 first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.speed > 0.0);
5376 let default_first_negative_steer =
5377 first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.steer < 0.0);
5378 let expanded_first_negative_steer =
5379 first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.steer < 0.0);
5380 let default_reacquired_near_goal =
5381 first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.near_goal);
5382 let expanded_reacquired_near_goal =
5383 first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.near_goal);
5384
5385 assert_eq!(default_first_stopped, Some(333));
5386 assert_eq!(expanded_first_stopped, Some(329));
5387 assert_eq!(default_first_positive_speed, Some(334));
5388 assert_eq!(expanded_first_positive_speed, Some(330));
5389 assert_eq!(default_first_negative_steer, Some(334));
5390 assert_eq!(expanded_first_negative_steer, Some(330));
5391 assert_eq!(default_reacquired_near_goal, Some(339));
5392 assert_eq!(expanded_reacquired_near_goal, Some(335));
5393 assert_eq!(
5394 default_first_positive_speed.unwrap() - default_first_stopped.unwrap(),
5395 1
5396 );
5397 assert_eq!(
5398 expanded_first_positive_speed.unwrap() - expanded_first_stopped.unwrap(),
5399 1
5400 );
5401 assert_eq!(
5402 default_reacquired_near_goal.unwrap() - default_first_positive_speed.unwrap(),
5403 5
5404 );
5405 assert_eq!(
5406 expanded_reacquired_near_goal.unwrap() - expanded_first_positive_speed.unwrap(),
5407 5
5408 );
5409 assert_eq!(
5410 default_first_positive_speed.unwrap() - expanded_first_positive_speed.unwrap(),
5411 4
5412 );
5413 assert_eq!(
5414 default_reacquired_near_goal.unwrap() - expanded_reacquired_near_goal.unwrap(),
5415 4
5416 );
5417
5418 let default_stop = goal_control_snapshot_at_step(&default_snapshots, 333);
5419 let expanded_stop = goal_control_snapshot_at_step(&expanded_snapshots, 329);
5420 let default_turn = goal_control_snapshot_at_step(&default_snapshots, 334);
5421 let expanded_turn = goal_control_snapshot_at_step(&expanded_snapshots, 330);
5422 let default_reentry = goal_control_snapshot_at_step(&default_snapshots, 339);
5423 let expanded_reentry = goal_control_snapshot_at_step(&expanded_snapshots, 335);
5424 let default_refinement = goal_control_snapshot_at_step(&default_snapshots, 346);
5425 let expanded_refinement = goal_control_snapshot_at_step(&expanded_snapshots, 342);
5426
5427 assert_goal_control_alignment(default_stop, expanded_stop);
5428 assert_goal_control_alignment(default_turn, expanded_turn);
5429 assert_goal_control_alignment(default_reentry, expanded_reentry);
5430 assert_goal_control_alignment(default_refinement, expanded_refinement);
5431
5432 assert!(default_stop.speed < 0.0);
5433 assert!(expanded_stop.speed < 0.0);
5434 assert!(default_stop.steer > 0.0);
5435 assert!(expanded_stop.steer > 0.0);
5436 assert!(default_stop.motion_curvature > 0.0);
5437 assert!(expanded_stop.motion_curvature > 0.0);
5438
5439 assert!(default_turn.speed > 0.0);
5440 assert!(expanded_turn.speed > 0.0);
5441 assert!(default_turn.steer < 0.0);
5442 assert!(expanded_turn.steer < 0.0);
5443 assert!(default_turn.motion_curvature < 0.0);
5444 assert!(expanded_turn.motion_curvature < 0.0);
5445 assert!(default_turn.course_curvature > 0.0);
5446 assert!(expanded_turn.course_curvature > 0.0);
5447
5448 assert!(default_reentry.near_goal);
5449 assert!(expanded_reentry.near_goal);
5450 assert!(default_reentry.speed > 1.0);
5451 assert!(expanded_reentry.speed > 1.0);
5452 assert!(default_reentry.motion_curvature < 0.0);
5453 assert!(expanded_reentry.motion_curvature < 0.0);
5454
5455 assert!(default_refinement.near_goal);
5456 assert!(expanded_refinement.near_goal);
5457 assert!(!default_refinement.stopped);
5458 assert!(!expanded_refinement.stopped);
5459 }
5460
5461 #[test]
5462 #[ignore = "long-running regression scenario"]
5463 fn test_mpc_switch_back_full_candidate_expansion_characterizes_terminal_planned_phase_lead() {
5464 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5465 346,
5466 optimize_linearized_controls_original,
5467 );
5468 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5469 342,
5470 optimize_linearized_controls_with_full_candidate_expansion,
5471 );
5472
5473 let default_stop = detailed_row_at_step(&default_trace, 332);
5474 let expanded_stop = detailed_row_at_step(&expanded_trace, 328);
5475 let default_turn = detailed_row_at_step(&default_trace, 333);
5476 let expanded_turn = detailed_row_at_step(&expanded_trace, 329);
5477 let default_reentry = detailed_row_at_step(&default_trace, 338);
5478 let expanded_reentry = detailed_row_at_step(&expanded_trace, 334);
5479 let default_refinement = detailed_row_at_step(&default_trace, 345);
5480 let expanded_refinement = detailed_row_at_step(&expanded_trace, 341);
5481
5482 assert_detailed_phase_alignment("terminal_stop", default_stop, expanded_stop);
5483 assert_detailed_phase_alignment("terminal_turn", default_turn, expanded_turn);
5484 assert_detailed_phase_alignment("terminal_reentry", default_reentry, expanded_reentry);
5485 assert_detailed_phase_alignment(
5486 "terminal_refinement",
5487 default_refinement,
5488 expanded_refinement,
5489 );
5490
5491 assert!(default_stop.controls[0][1] > 0.0);
5492 assert!(expanded_stop.controls[0][1] > 0.0);
5493 assert!(default_turn.controls[0][1] < 0.0);
5494 assert!(expanded_turn.controls[0][1] < 0.0);
5495 assert!(default_reentry.controls[0][0] > 0.2);
5496 assert!(expanded_reentry.controls[0][0] > 0.2);
5497 assert!(default_refinement.controls[0][0] < 0.0);
5498 assert!(expanded_refinement.controls[0][0] < 0.0);
5499 }
5500
5501 #[test]
5502 #[ignore = "long-running regression scenario"]
5503 fn test_mpc_switch_back_full_candidate_expansion_characterizes_pre_terminal_phase_origin() {
5504 let checkpoints = [321usize, 322, 323, 324, 325, 326, 327, 328, 329];
5505 let default_snapshots = collect_switch_back_planning_snapshots_with_test_optimizer(
5506 330,
5507 &checkpoints,
5508 optimize_linearized_controls_original,
5509 );
5510 let expanded_snapshots = collect_switch_back_planning_snapshots_with_test_optimizer(
5511 330,
5512 &checkpoints,
5513 optimize_linearized_controls_with_full_candidate_expansion,
5514 );
5515
5516 let same_325_default = planning_snapshot_at_step(&default_snapshots, 325);
5517 let same_325_expanded = planning_snapshot_at_step(&expanded_snapshots, 325);
5518 let same_328_default = planning_snapshot_at_step(&default_snapshots, 328);
5519 let same_328_expanded = planning_snapshot_at_step(&expanded_snapshots, 328);
5520 let same_329_default = planning_snapshot_at_step(&default_snapshots, 329);
5521 let same_329_expanded = planning_snapshot_at_step(&expanded_snapshots, 329);
5522
5523 assert_eq!(same_325_default.target_index, 161);
5524 assert_eq!(same_325_expanded.target_index, 161);
5525 assert_diff_bounds(
5526 "pre_terminal_same_325_xref_head",
5527 trace_state_diff(&same_325_default.xref_head, &same_325_expanded.xref_head),
5528 [1e-10; 4],
5529 );
5530 assert_diff_bounds(
5531 "pre_terminal_same_328_xref_head",
5532 trace_state_diff(&same_328_default.xref_head, &same_328_expanded.xref_head),
5533 [1e-10; 4],
5534 );
5535 assert_diff_bounds(
5536 "pre_terminal_same_329_xref_head",
5537 trace_state_diff(&same_329_default.xref_head, &same_329_expanded.xref_head),
5538 [1e-10; 4],
5539 );
5540
5541 assert!(same_325_default.state[1] - same_325_expanded.state[1] > 1.0);
5542 assert!(same_325_expanded.state[2] - same_325_default.state[2] > 0.79);
5543 assert!(same_328_default.state[1] - same_328_expanded.state[1] > 0.55);
5544 assert!(same_328_expanded.state[2] - same_328_default.state[2] > 0.79);
5545 assert!(same_325_default.warm_start_head[1] < 0.0);
5546 assert!(same_325_expanded.warm_start_head[1] > 0.0);
5547 assert!(same_328_default.warm_start_head[1] < 0.0);
5548 assert!(same_328_expanded.warm_start_head[1] > 0.0);
5549 assert!(same_329_default.warm_start_head[1] > 0.0);
5550 assert!(same_329_expanded.warm_start_head[1] > 0.0);
5551
5552 for (default_step, expanded_step) in [
5553 (325usize, 321usize),
5554 (326, 322),
5555 (327, 323),
5556 (328, 324),
5557 (329, 325),
5558 ] {
5559 let default = planning_snapshot_at_step(&default_snapshots, default_step);
5560 let expanded = planning_snapshot_at_step(&expanded_snapshots, expanded_step);
5561 assert_planning_snapshot_phase_alignment(
5562 &format!("pre_terminal_shifted_{default_step}_{expanded_step}"),
5563 default,
5564 expanded,
5565 );
5566 }
5567 }
5568
5569 #[test]
5570 #[ignore = "long-running regression scenario"]
5571 fn test_mpc_switch_back_full_candidate_expansion_characterizes_shifted_warm_start_injection() {
5572 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5573 334,
5574 optimize_linearized_controls_original,
5575 );
5576 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5577 330,
5578 optimize_linearized_controls_with_full_candidate_expansion,
5579 );
5580 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5581 334,
5582 optimize_linearized_controls_original,
5583 );
5584 let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5585 330,
5586 optimize_linearized_controls_with_full_candidate_expansion,
5587 );
5588
5589 for (default_step, expanded_step) in [
5590 (325usize, 321usize),
5591 (326, 322),
5592 (327, 323),
5593 (328, 324),
5594 (329, 325),
5595 ] {
5596 let default_row = detailed_row_at_step(&default_trace, default_step);
5597 let expanded_row = detailed_row_at_step(&expanded_trace, default_step);
5598 let injected_default = iterative_linear_mpc_control_with_test_optimizer(
5599 &default_row.xref,
5600 &state_from_vector4(&default_row.state),
5601 expanded_warm_start.get(&expanded_step).unwrap(),
5602 optimize_linearized_controls_original,
5603 );
5604 let injected_expanded = iterative_linear_mpc_control_with_test_optimizer(
5605 &expanded_row.xref,
5606 &state_from_vector4(&expanded_row.state),
5607 default_warm_start.get(&(default_step + 4)).unwrap(),
5608 optimize_linearized_controls_with_full_candidate_expansion,
5609 );
5610
5611 let default_control_rows = default_row
5612 .controls
5613 .iter()
5614 .map(|vec| [vec[0], vec[1]])
5615 .collect::<Vec<_>>();
5616 let default_predicted_rows = default_row
5617 .predicted
5618 .iter()
5619 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5620 .collect::<Vec<_>>();
5621 let expanded_control_rows = expanded_row
5622 .controls
5623 .iter()
5624 .map(|vec| [vec[0], vec[1]])
5625 .collect::<Vec<_>>();
5626 let expanded_predicted_rows = expanded_row
5627 .predicted
5628 .iter()
5629 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5630 .collect::<Vec<_>>();
5631
5632 assert_diff_bounds(
5633 &format!("shifted_warm_default_step_{default_step}_control"),
5634 summarize_vec2_sequence_diff(&injected_default.controls, &default_control_rows),
5635 [1e-3, 6e-4],
5636 );
5637 assert_diff_bounds(
5638 &format!("shifted_warm_default_step_{default_step}_predicted"),
5639 summarize_vec4_sequence_diff(&injected_default.predicted, &default_predicted_rows),
5640 [1.5e-3, 1.5e-3, 4e-4, 4e-4],
5641 );
5642 assert_diff_bounds(
5643 &format!("shifted_warm_expanded_step_{default_step}_control"),
5644 summarize_vec2_sequence_diff(&injected_expanded.controls, &expanded_control_rows),
5645 [1e-3, 6e-4],
5646 );
5647 assert_diff_bounds(
5648 &format!("shifted_warm_expanded_step_{default_step}_predicted"),
5649 summarize_vec4_sequence_diff(
5650 &injected_expanded.predicted,
5651 &expanded_predicted_rows,
5652 ),
5653 [1.5e-3, 1.5e-3, 4e-4, 4e-4],
5654 );
5655 }
5656 }
5657
5658 #[test]
5659 #[ignore = "long-running regression scenario"]
5660 fn test_mpc_switch_back_full_candidate_expansion_characterizes_same_time_warm_start_injection()
5661 {
5662 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5663 330,
5664 optimize_linearized_controls_original,
5665 );
5666 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5667 330,
5668 optimize_linearized_controls_with_full_candidate_expansion,
5669 );
5670 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5671 330,
5672 optimize_linearized_controls_original,
5673 );
5674 let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5675 330,
5676 optimize_linearized_controls_with_full_candidate_expansion,
5677 );
5678
5679 for step in [325usize, 326, 327, 328, 329] {
5680 let default_row = detailed_row_at_step(&default_trace, step);
5681 let expanded_row = detailed_row_at_step(&expanded_trace, step);
5682 let default_with_expanded_warm = iterative_linear_mpc_control_with_test_optimizer(
5683 &default_row.xref,
5684 &state_from_vector4(&default_row.state),
5685 expanded_warm_start.get(&step).unwrap(),
5686 optimize_linearized_controls_original,
5687 );
5688 let expanded_with_default_warm = iterative_linear_mpc_control_with_test_optimizer(
5689 &expanded_row.xref,
5690 &state_from_vector4(&expanded_row.state),
5691 default_warm_start.get(&step).unwrap(),
5692 optimize_linearized_controls_with_full_candidate_expansion,
5693 );
5694
5695 let default_baseline_controls = default_row
5696 .controls
5697 .iter()
5698 .map(|vec| [vec[0], vec[1]])
5699 .collect::<Vec<_>>();
5700 let expanded_baseline_controls = expanded_row
5701 .controls
5702 .iter()
5703 .map(|vec| [vec[0], vec[1]])
5704 .collect::<Vec<_>>();
5705 let default_baseline_predicted = default_row
5706 .predicted
5707 .iter()
5708 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5709 .collect::<Vec<_>>();
5710 let expanded_baseline_predicted = expanded_row
5711 .predicted
5712 .iter()
5713 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5714 .collect::<Vec<_>>();
5715
5716 let default_to_default_control = summarize_vec2_sequence_diff(
5717 &default_with_expanded_warm.controls,
5718 &default_baseline_controls,
5719 );
5720 let default_to_expanded_control = summarize_vec2_sequence_diff(
5721 &default_with_expanded_warm.controls,
5722 &expanded_baseline_controls,
5723 );
5724 let default_to_default_predicted = summarize_vec4_sequence_diff(
5725 &default_with_expanded_warm.predicted,
5726 &default_baseline_predicted,
5727 );
5728 let default_to_expanded_predicted = summarize_vec4_sequence_diff(
5729 &default_with_expanded_warm.predicted,
5730 &expanded_baseline_predicted,
5731 );
5732 let expanded_to_expanded_control = summarize_vec2_sequence_diff(
5733 &expanded_with_default_warm.controls,
5734 &expanded_baseline_controls,
5735 );
5736 let expanded_to_default_control = summarize_vec2_sequence_diff(
5737 &expanded_with_default_warm.controls,
5738 &default_baseline_controls,
5739 );
5740 let expanded_to_expanded_predicted = summarize_vec4_sequence_diff(
5741 &expanded_with_default_warm.predicted,
5742 &expanded_baseline_predicted,
5743 );
5744 let expanded_to_default_predicted = summarize_vec4_sequence_diff(
5745 &expanded_with_default_warm.predicted,
5746 &default_baseline_predicted,
5747 );
5748
5749 assert_diff_bounds(
5750 &format!("same_time_warm_default_step_{step}_control"),
5751 default_to_default_control,
5752 [1.6e-2, 1.1e-2],
5753 );
5754 assert_diff_bounds(
5755 &format!("same_time_warm_default_step_{step}_predicted"),
5756 default_to_default_predicted,
5757 [2.5e-3, 3e-4, 3.5e-3, 5e-3],
5758 );
5759 assert!(
5760 default_to_expanded_control[0] > 0.7,
5761 "same-time default branch drifted toward expanded control unexpectedly: step={} same={:?} opposite={:?}",
5762 step,
5763 default_to_default_control,
5764 default_to_expanded_control
5765 );
5766 assert!(
5767 default_to_expanded_predicted[1] > 0.4
5768 && default_to_expanded_predicted[2] > 0.79,
5769 "same-time default branch drifted toward expanded prediction unexpectedly: step={} same={:?} opposite={:?}",
5770 step,
5771 default_to_default_predicted,
5772 default_to_expanded_predicted
5773 );
5774
5775 assert_diff_bounds(
5776 &format!("same_time_warm_expanded_step_{step}_control"),
5777 expanded_to_expanded_control,
5778 [6.5e-3, 3.6e-3],
5779 );
5780 assert_diff_bounds(
5781 &format!("same_time_warm_expanded_step_{step}_predicted"),
5782 expanded_to_expanded_predicted,
5783 [1e-4, 3e-4, 1.3e-3, 6e-4],
5784 );
5785 assert!(
5786 expanded_to_default_control[0] > 0.7,
5787 "same-time expanded branch drifted toward default control unexpectedly: step={} same={:?} opposite={:?}",
5788 step,
5789 expanded_to_expanded_control,
5790 expanded_to_default_control
5791 );
5792 assert!(
5793 expanded_to_default_predicted[1] > 0.4
5794 && expanded_to_default_predicted[2] > 0.79,
5795 "same-time expanded branch drifted toward default prediction unexpectedly: step={} same={:?} opposite={:?}",
5796 step,
5797 expanded_to_expanded_predicted,
5798 expanded_to_default_predicted
5799 );
5800 }
5801 }
5802
5803 #[test]
5804 #[ignore = "long-running regression scenario"]
5805 fn test_mpc_switch_back_full_candidate_expansion_characterizes_same_time_state_injection() {
5806 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5807 330,
5808 optimize_linearized_controls_original,
5809 );
5810 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5811 330,
5812 optimize_linearized_controls_with_full_candidate_expansion,
5813 );
5814 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5815 330,
5816 optimize_linearized_controls_original,
5817 );
5818 let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5819 330,
5820 optimize_linearized_controls_with_full_candidate_expansion,
5821 );
5822
5823 for step in [325usize, 326, 327, 328, 329] {
5824 let default_row = detailed_row_at_step(&default_trace, step);
5825 let expanded_row = detailed_row_at_step(&expanded_trace, step);
5826 let default_with_expanded_state = iterative_linear_mpc_control_with_test_optimizer(
5827 &default_row.xref,
5828 &state_from_vector4(&expanded_row.state),
5829 default_warm_start.get(&step).unwrap(),
5830 optimize_linearized_controls_original,
5831 );
5832 let expanded_with_default_state = iterative_linear_mpc_control_with_test_optimizer(
5833 &expanded_row.xref,
5834 &state_from_vector4(&default_row.state),
5835 expanded_warm_start.get(&step).unwrap(),
5836 optimize_linearized_controls_with_full_candidate_expansion,
5837 );
5838
5839 let default_baseline_controls = default_row
5840 .controls
5841 .iter()
5842 .map(|vec| [vec[0], vec[1]])
5843 .collect::<Vec<_>>();
5844 let expanded_baseline_controls = expanded_row
5845 .controls
5846 .iter()
5847 .map(|vec| [vec[0], vec[1]])
5848 .collect::<Vec<_>>();
5849 let default_baseline_predicted = default_row
5850 .predicted
5851 .iter()
5852 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5853 .collect::<Vec<_>>();
5854 let expanded_baseline_predicted = expanded_row
5855 .predicted
5856 .iter()
5857 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5858 .collect::<Vec<_>>();
5859
5860 let default_to_default_control = summarize_vec2_sequence_diff(
5861 &default_with_expanded_state.controls,
5862 &default_baseline_controls,
5863 );
5864 let default_to_expanded_control = summarize_vec2_sequence_diff(
5865 &default_with_expanded_state.controls,
5866 &expanded_baseline_controls,
5867 );
5868 let default_to_default_predicted = summarize_vec4_sequence_diff(
5869 &default_with_expanded_state.predicted,
5870 &default_baseline_predicted,
5871 );
5872 let default_to_expanded_predicted = summarize_vec4_sequence_diff(
5873 &default_with_expanded_state.predicted,
5874 &expanded_baseline_predicted,
5875 );
5876 let expanded_to_expanded_control = summarize_vec2_sequence_diff(
5877 &expanded_with_default_state.controls,
5878 &expanded_baseline_controls,
5879 );
5880 let expanded_to_default_control = summarize_vec2_sequence_diff(
5881 &expanded_with_default_state.controls,
5882 &default_baseline_controls,
5883 );
5884 let expanded_to_expanded_predicted = summarize_vec4_sequence_diff(
5885 &expanded_with_default_state.predicted,
5886 &expanded_baseline_predicted,
5887 );
5888 let expanded_to_default_predicted = summarize_vec4_sequence_diff(
5889 &expanded_with_default_state.predicted,
5890 &default_baseline_predicted,
5891 );
5892
5893 assert!(
5894 default_to_default_control[0] > 0.5,
5895 "same-time default-state injection did not move control away from default baseline enough: step={} same={:?} opposite={:?}",
5896 step,
5897 default_to_default_control,
5898 default_to_expanded_control
5899 );
5900 assert!(
5901 default_to_default_predicted[1] > 0.35 && default_to_default_predicted[2] > 0.7,
5902 "same-time default-state injection did not move prediction away from default baseline enough: step={} same={:?} opposite={:?}",
5903 step,
5904 default_to_default_predicted,
5905 default_to_expanded_predicted
5906 );
5907 assert!(
5908 default_to_default_control[0] > 4.0 * default_to_expanded_control[0],
5909 "same-time default-state injection stayed too close to default control branch: step={} same={:?} opposite={:?}",
5910 step,
5911 default_to_default_control,
5912 default_to_expanded_control
5913 );
5914 assert!(
5915 default_to_default_predicted[1] > 4.0 * default_to_expanded_predicted[1]
5916 && default_to_default_predicted[2] > 4.0 * default_to_expanded_predicted[2],
5917 "same-time default-state injection stayed too close to default predicted branch: step={} same={:?} opposite={:?}",
5918 step,
5919 default_to_default_predicted,
5920 default_to_expanded_predicted
5921 );
5922
5923 assert!(
5924 expanded_to_expanded_control[0] > 0.5,
5925 "same-time expanded-state injection did not move control away from expanded baseline enough: step={} same={:?} opposite={:?}",
5926 step,
5927 expanded_to_expanded_control,
5928 expanded_to_default_control
5929 );
5930 assert!(
5931 expanded_to_expanded_predicted[1] > 0.35
5932 && expanded_to_expanded_predicted[2] > 0.7,
5933 "same-time expanded-state injection did not move prediction away from expanded baseline enough: step={} same={:?} opposite={:?}",
5934 step,
5935 expanded_to_expanded_predicted,
5936 expanded_to_default_predicted
5937 );
5938 assert!(
5939 expanded_to_expanded_control[0] > 4.0 * expanded_to_default_control[0],
5940 "same-time expanded-state injection stayed too close to expanded control branch: step={} same={:?} opposite={:?}",
5941 step,
5942 expanded_to_expanded_control,
5943 expanded_to_default_control
5944 );
5945 assert!(
5946 expanded_to_expanded_predicted[1] > 4.0 * expanded_to_default_predicted[1]
5947 && expanded_to_expanded_predicted[2] > 4.0 * expanded_to_default_predicted[2],
5948 "same-time expanded-state injection stayed too close to expanded predicted branch: step={} same={:?} opposite={:?}",
5949 step,
5950 expanded_to_expanded_predicted,
5951 expanded_to_default_predicted
5952 );
5953 }
5954 }
5955
5956 #[test]
5957 #[ignore = "long-running regression scenario"]
5958 fn test_mpc_switch_back_full_candidate_expansion_characterizes_state_component_injection() {
5959 let x_metrics = measure_switch_back_state_injection(&[0usize]);
5960 let y_metrics = measure_switch_back_state_injection(&[1usize]);
5961 let v_metrics = measure_switch_back_state_injection(&[2usize]);
5962 let yaw_metrics = measure_switch_back_state_injection(&[3usize]);
5963
5964 assert!(
5965 x_metrics.default_same_control0 < x_metrics.default_opposite_control0 / 100.0
5966 && x_metrics.default_same_pred_y < x_metrics.default_opposite_pred_y / 100.0
5967 && x_metrics.default_same_pred_v < x_metrics.default_opposite_pred_v / 100.0,
5968 "x-only injection moved the default branch more than expected: {:?}",
5969 x_metrics
5970 );
5971 assert!(
5972 x_metrics.expanded_same_control0 < x_metrics.expanded_opposite_control0 / 100.0
5973 && x_metrics.expanded_same_pred_y < x_metrics.expanded_opposite_pred_y / 100.0
5974 && x_metrics.expanded_same_pred_v < x_metrics.expanded_opposite_pred_v / 100.0,
5975 "x-only injection moved the expanded branch more than expected: {:?}",
5976 x_metrics
5977 );
5978
5979 assert!(
5980 yaw_metrics.default_same_control0 < yaw_metrics.default_opposite_control0 / 100.0
5981 && yaw_metrics.default_same_pred_y < yaw_metrics.default_opposite_pred_y / 100.0
5982 && yaw_metrics.default_same_pred_v < yaw_metrics.default_opposite_pred_v / 100.0,
5983 "yaw-only injection moved the default branch more than expected: {:?}",
5984 yaw_metrics
5985 );
5986 assert!(
5987 yaw_metrics.expanded_same_control0 < yaw_metrics.expanded_opposite_control0 / 100.0
5988 && yaw_metrics.expanded_same_pred_y < yaw_metrics.expanded_opposite_pred_y / 100.0
5989 && yaw_metrics.expanded_same_pred_v < yaw_metrics.expanded_opposite_pred_v / 100.0,
5990 "yaw-only injection moved the expanded branch more than expected: {:?}",
5991 yaw_metrics
5992 );
5993
5994 assert!(
5995 y_metrics.default_same_control0 < y_metrics.default_opposite_control0 / 4.0
5996 && y_metrics.default_same_pred_v < y_metrics.default_opposite_pred_v / 4.0
5997 && y_metrics.default_same_pred_y > 3.0,
5998 "y-only injection changed the default branch characterization unexpectedly: {:?}",
5999 y_metrics
6000 );
6001 assert!(
6002 y_metrics.expanded_same_control0 < y_metrics.expanded_opposite_control0 / 4.0
6003 && y_metrics.expanded_same_pred_v < y_metrics.expanded_opposite_pred_v / 4.0
6004 && y_metrics.expanded_same_pred_y > 3.0,
6005 "y-only injection changed the expanded branch characterization unexpectedly: {:?}",
6006 y_metrics
6007 );
6008
6009 assert!(
6010 v_metrics.default_same_control0 > 4.0 * v_metrics.default_opposite_control0
6011 && v_metrics.default_same_pred_v > 4.0 * v_metrics.default_opposite_pred_v,
6012 "v-only injection did not pull the default branch toward the opposite phase enough: {:?}",
6013 v_metrics
6014 );
6015 assert!(
6016 v_metrics.expanded_same_control0 > 4.0 * v_metrics.expanded_opposite_control0
6017 && v_metrics.expanded_same_pred_v > 4.0 * v_metrics.expanded_opposite_pred_v,
6018 "v-only injection did not pull the expanded branch toward the opposite phase enough: {:?}",
6019 v_metrics
6020 );
6021 }
6022
6023 #[test]
6024 #[ignore = "long-running regression scenario"]
6025 fn test_mpc_switch_back_full_candidate_expansion_characterizes_state_subset_injection() {
6026 let vy_metrics = measure_switch_back_state_injection(&[1usize, 2usize]);
6027 let vyyaw_metrics = measure_switch_back_state_injection(&[1usize, 2usize, 3usize]);
6028
6029 assert!(
6030 vy_metrics.default_same_control0 > 100.0 * vy_metrics.default_opposite_control0
6031 && vy_metrics.default_same_pred_y > 100.0 * vy_metrics.default_opposite_pred_y
6032 && vy_metrics.default_same_pred_v > 100.0 * vy_metrics.default_opposite_pred_v,
6033 "v+y injection did not pull the default branch strongly enough toward the opposite phase: {:?}",
6034 vy_metrics
6035 );
6036 assert!(
6037 vy_metrics.expanded_same_control0 > 100.0 * vy_metrics.expanded_opposite_control0
6038 && vy_metrics.expanded_same_pred_y > 100.0 * vy_metrics.expanded_opposite_pred_y
6039 && vy_metrics.expanded_same_pred_v > 100.0 * vy_metrics.expanded_opposite_pred_v,
6040 "v+y injection did not pull the expanded branch strongly enough toward the opposite phase: {:?}",
6041 vy_metrics
6042 );
6043
6044 assert!(
6045 vyyaw_metrics.default_same_control0 > 100.0 * vyyaw_metrics.default_opposite_control0
6046 && vyyaw_metrics.default_same_pred_y
6047 > 100.0 * vyyaw_metrics.default_opposite_pred_y
6048 && vyyaw_metrics.default_same_pred_v
6049 > 100.0 * vyyaw_metrics.default_opposite_pred_v,
6050 "v+y+yaw injection did not pull the default branch strongly enough toward the opposite phase: {:?}",
6051 vyyaw_metrics
6052 );
6053 assert!(
6054 vyyaw_metrics.expanded_same_control0
6055 > 100.0 * vyyaw_metrics.expanded_opposite_control0
6056 && vyyaw_metrics.expanded_same_pred_y
6057 > 100.0 * vyyaw_metrics.expanded_opposite_pred_y
6058 && vyyaw_metrics.expanded_same_pred_v
6059 > 100.0 * vyyaw_metrics.expanded_opposite_pred_v,
6060 "v+y+yaw injection did not pull the expanded branch strongly enough toward the opposite phase: {:?}",
6061 vyyaw_metrics
6062 );
6063
6064 assert!(
6065 (vy_metrics.default_same_control0 - vyyaw_metrics.default_same_control0).abs() <= 1e-3
6066 && (vy_metrics.default_opposite_control0
6067 - vyyaw_metrics.default_opposite_control0)
6068 .abs()
6069 <= 1e-3
6070 && (vy_metrics.default_opposite_pred_y - vyyaw_metrics.default_opposite_pred_y)
6071 .abs()
6072 <= 1e-4
6073 && (vy_metrics.default_opposite_pred_v - vyyaw_metrics.default_opposite_pred_v)
6074 .abs()
6075 <= 2e-4,
6076 "adding yaw to v+y changed the default-branch subset metrics more than expected: vy={:?} vyyaw={:?}",
6077 vy_metrics,
6078 vyyaw_metrics
6079 );
6080 assert!(
6081 (vy_metrics.expanded_same_control0 - vyyaw_metrics.expanded_same_control0).abs()
6082 <= 1e-3
6083 && (vy_metrics.expanded_opposite_control0
6084 - vyyaw_metrics.expanded_opposite_control0)
6085 .abs()
6086 <= 1e-3
6087 && (vy_metrics.expanded_opposite_pred_y - vyyaw_metrics.expanded_opposite_pred_y)
6088 .abs()
6089 <= 1e-4
6090 && (vy_metrics.expanded_opposite_pred_v - vyyaw_metrics.expanded_opposite_pred_v)
6091 .abs()
6092 <= 2e-4,
6093 "adding yaw to v+y changed the expanded-branch subset metrics more than expected: vy={:?} vyyaw={:?}",
6094 vy_metrics,
6095 vyyaw_metrics
6096 );
6097 }
6098
6099 #[test]
6100 #[ignore = "long-running regression scenario"]
6101 fn test_mpc_switch_back_full_candidate_expansion_characterizes_state_subset_per_step() {
6102 let steps = [325usize, 326, 327, 328, 329];
6103 let v_metrics = measure_switch_back_state_injection_by_step(&[2usize], &steps);
6104 let vy_metrics = measure_switch_back_state_injection_by_step(&[1usize, 2usize], &steps);
6105
6106 let step_325_v = state_injection_metrics_at_step(&v_metrics, 325);
6107 let step_325_vy = state_injection_metrics_at_step(&vy_metrics, 325);
6108 assert!(
6109 (step_325_v.default_opposite_control0 - step_325_vy.default_opposite_control0).abs()
6110 <= 5e-4,
6111 "default branch control moved too early at step 325: v={:?} vy={:?}",
6112 step_325_v,
6113 step_325_vy
6114 );
6115 assert!(
6116 step_325_v.default_opposite_pred_y > 10_000.0 * step_325_vy.default_opposite_pred_y,
6117 "default branch predicted-y should already need y at step 325: v={:?} vy={:?}",
6118 step_325_v,
6119 step_325_vy
6120 );
6121 assert!(
6122 step_325_v.expanded_opposite_pred_y > 1_000.0 * step_325_vy.expanded_opposite_pred_y,
6123 "expanded branch predicted-y should already improve with y at step 325: v={:?} vy={:?}",
6124 step_325_v,
6125 step_325_vy
6126 );
6127
6128 for step in [326usize, 327, 328, 329] {
6129 let v = state_injection_metrics_at_step(&v_metrics, step);
6130 let vy = state_injection_metrics_at_step(&vy_metrics, step);
6131
6132 assert!(
6133 v.default_opposite_control0 > 20.0 * vy.default_opposite_control0,
6134 "default branch control should need y from step {step}: v={:?} vy={:?}",
6135 v,
6136 vy
6137 );
6138 assert!(
6139 v.default_opposite_pred_y > 1_000.0 * vy.default_opposite_pred_y,
6140 "default branch predicted-y should strongly prefer v+y from step {step}: v={:?} vy={:?}",
6141 v,
6142 vy
6143 );
6144 assert!(
6145 v.expanded_opposite_pred_y > 1_000.0 * vy.expanded_opposite_pred_y,
6146 "expanded branch predicted-y should strongly prefer v+y from step {step}: v={:?} vy={:?}",
6147 v,
6148 vy
6149 );
6150 }
6151
6152 for step in [325usize, 326, 327, 328, 329] {
6153 let v = state_injection_metrics_at_step(&v_metrics, step);
6154 let vy = state_injection_metrics_at_step(&vy_metrics, step);
6155 assert!(
6156 (v.expanded_opposite_control0 - vy.expanded_opposite_control0).abs() <= 3e-3,
6157 "expanded branch control should stay mostly v-driven at step {step}: v={:?} vy={:?}",
6158 v,
6159 vy
6160 );
6161 assert!(
6162 (v.expanded_opposite_pred_v - vy.expanded_opposite_pred_v).abs() <= 6e-4,
6163 "expanded branch predicted-v should stay mostly v-driven at step {step}: v={:?} vy={:?}",
6164 v,
6165 vy
6166 );
6167 }
6168 }
6169
6170 #[derive(Debug)]
6172 struct StepTransitionDeltaMetrics {
6173 state_delta: Vector4<f64>,
6175 baseline_control0: [f64; 2],
6177 expanded_baseline_control0: [f64; 2],
6179 injections: Vec<DeltaInjectionResult>,
6181 }
6182
6183 #[derive(Debug)]
6184 struct DeltaInjectionResult {
6185 label: &'static str,
6186 indices: Vec<usize>,
6187 injected_control0: [f64; 2],
6188 default_same_control0: f64,
6189 default_opposite_control0: f64,
6190 default_same_pred_y: f64,
6191 default_opposite_pred_y: f64,
6192 default_same_pred_v: f64,
6193 default_opposite_pred_v: f64,
6194 }
6195
6196 fn measure_step_transition_delta_injection() -> StepTransitionDeltaMetrics {
6197 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6198 330,
6199 optimize_linearized_controls_original,
6200 );
6201 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6202 330,
6203 optimize_linearized_controls_with_full_candidate_expansion,
6204 );
6205 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
6206 330,
6207 optimize_linearized_controls_original,
6208 );
6209
6210 let row_325 = detailed_row_at_step(&default_trace, 325);
6211 let row_326 = detailed_row_at_step(&default_trace, 326);
6212 let expanded_row_325 = detailed_row_at_step(&expanded_trace, 325);
6213
6214 let state_delta = row_326.state - row_325.state;
6215
6216 let default_baseline_controls: Vec<[f64; 2]> = row_325
6217 .controls
6218 .iter()
6219 .map(|vec| [vec[0], vec[1]])
6220 .collect();
6221 let expanded_baseline_controls: Vec<[f64; 2]> = expanded_row_325
6222 .controls
6223 .iter()
6224 .map(|vec| [vec[0], vec[1]])
6225 .collect();
6226 let default_baseline_predicted: Vec<[f64; 4]> = row_325
6227 .predicted
6228 .iter()
6229 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6230 .collect();
6231 let expanded_baseline_predicted: Vec<[f64; 4]> = expanded_row_325
6232 .predicted
6233 .iter()
6234 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6235 .collect();
6236
6237 let component_sets: Vec<(&str, Vec<usize>)> = vec![
6238 ("delta_v", vec![2]),
6239 ("delta_y", vec![1]),
6240 ("delta_v+delta_y", vec![1, 2]),
6241 ("delta_v+delta_y+delta_yaw", vec![1, 2, 3]),
6242 ];
6243
6244 let warm_start_325 = default_warm_start.get(&325).unwrap();
6245
6246 let mut injections = Vec::new();
6247 for (label, indices) in &component_sets {
6248 let mut modified_state_vec = row_325.state;
6249 for &idx in indices {
6250 modified_state_vec[idx] += state_delta[idx];
6251 }
6252 let modified_state = state_from_vector4(&modified_state_vec);
6253
6254 let result = iterative_linear_mpc_control_with_test_optimizer(
6255 &row_325.xref,
6256 &modified_state,
6257 warm_start_325,
6258 optimize_linearized_controls_original,
6259 );
6260
6261 let to_default_control =
6262 summarize_vec2_sequence_diff(&result.controls, &default_baseline_controls);
6263 let to_expanded_control =
6264 summarize_vec2_sequence_diff(&result.controls, &expanded_baseline_controls);
6265 let to_default_predicted =
6266 summarize_vec4_sequence_diff(&result.predicted, &default_baseline_predicted);
6267 let to_expanded_predicted =
6268 summarize_vec4_sequence_diff(&result.predicted, &expanded_baseline_predicted);
6269
6270 injections.push(DeltaInjectionResult {
6271 label,
6272 indices: indices.clone(),
6273 injected_control0: [result.controls[0][0], result.controls[0][1]],
6274 default_same_control0: to_default_control[0],
6275 default_opposite_control0: to_expanded_control[0],
6276 default_same_pred_y: to_default_predicted[1],
6277 default_opposite_pred_y: to_expanded_predicted[1],
6278 default_same_pred_v: to_default_predicted[2],
6279 default_opposite_pred_v: to_expanded_predicted[2],
6280 });
6281 }
6282
6283 StepTransitionDeltaMetrics {
6284 state_delta,
6285 baseline_control0: [row_325.controls[0][0], row_325.controls[0][1]],
6286 expanded_baseline_control0: [
6287 expanded_row_325.controls[0][0],
6288 expanded_row_325.controls[0][1],
6289 ],
6290 injections,
6291 }
6292 }
6293
6294 #[test]
6295 #[ignore = "long-running regression scenario"]
6296 fn test_mpc_switch_back_full_candidate_expansion_characterizes_step_325_326_transition() {
6297 let metrics = measure_step_transition_delta_injection();
6298
6299 println!("=== Step 325 → 326 Transition Delta Injection ===");
6300 println!("state_delta: {:?}", metrics.state_delta);
6301 println!(
6302 "baseline_control0 (default): {:?}",
6303 metrics.baseline_control0
6304 );
6305 println!(
6306 "baseline_control0 (expanded): {:?}",
6307 metrics.expanded_baseline_control0
6308 );
6309 for inj in &metrics.injections {
6310 println!("--- {} (indices {:?}) ---", inj.label, inj.indices);
6311 println!(" injected_control0: {:?}", inj.injected_control0);
6312 println!(
6313 " same_control0: {:.6e} opposite_control0: {:.6e} ratio: {:.2}",
6314 inj.default_same_control0,
6315 inj.default_opposite_control0,
6316 if inj.default_opposite_control0 > 0.0 {
6317 inj.default_same_control0 / inj.default_opposite_control0
6318 } else {
6319 f64::INFINITY
6320 }
6321 );
6322 println!(
6323 " same_pred_y: {:.6e} opposite_pred_y: {:.6e}",
6324 inj.default_same_pred_y, inj.default_opposite_pred_y
6325 );
6326 println!(
6327 " same_pred_v: {:.6e} opposite_pred_v: {:.6e}",
6328 inj.default_same_pred_v, inj.default_opposite_pred_v
6329 );
6330 }
6331
6332 let delta_v = &metrics.injections[0];
6335 assert!(
6336 delta_v.default_same_control0 < delta_v.default_opposite_control0,
6337 "delta_v at step 325 context should stay closer to default than expanded: {:?}",
6338 delta_v
6339 );
6340
6341 let delta_y = &metrics.injections[1];
6343 assert!(
6344 delta_y.default_same_control0 < delta_y.default_opposite_control0,
6345 "delta_y at step 325 context should stay closer to default than expanded: {:?}",
6346 delta_y
6347 );
6348
6349 let delta_vy = &metrics.injections[2];
6354 println!(
6355 "\n=== KEY: delta_v+delta_y same/opposite ratio = {:.4} ===",
6356 if delta_vy.default_opposite_control0 > 0.0 {
6357 delta_vy.default_same_control0 / delta_vy.default_opposite_control0
6358 } else {
6359 f64::INFINITY
6360 }
6361 );
6362
6363 let delta_vyy = &metrics.injections[3];
6365 let control0_diff =
6366 (delta_vy.default_same_control0 - delta_vyy.default_same_control0).abs();
6367 println!(
6368 "delta_v+delta_y vs delta_v+delta_y+delta_yaw control0 diff: {:.6e}",
6369 control0_diff
6370 );
6371 }
6372
6373 #[derive(Debug)]
6375 struct ContextSwapInjectionResult {
6376 label: &'static str,
6377 injected_steer: f64,
6378 default_baseline_steer: f64,
6379 expanded_baseline_steer: f64,
6380 default_same_control0: f64,
6381 default_opposite_control0: f64,
6382 default_same_pred_y: f64,
6383 default_opposite_pred_y: f64,
6384 default_same_pred_v: f64,
6385 default_opposite_pred_v: f64,
6386 }
6387
6388 fn measure_step_325_326_context_swap_v_injection() -> Vec<ContextSwapInjectionResult> {
6392 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6393 330,
6394 optimize_linearized_controls_original,
6395 );
6396 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6397 330,
6398 optimize_linearized_controls_with_full_candidate_expansion,
6399 );
6400 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
6401 330,
6402 optimize_linearized_controls_original,
6403 );
6404
6405 let d325 = detailed_row_at_step(&default_trace, 325);
6406 let d326 = detailed_row_at_step(&default_trace, 326);
6407 let e325 = detailed_row_at_step(&expanded_trace, 325);
6408 let e326 = detailed_row_at_step(&expanded_trace, 326);
6409 let ws325 = default_warm_start.get(&325).unwrap();
6410 let ws326 = default_warm_start.get(&326).unwrap();
6411
6412 let v_indices: &[usize] = &[2]; let run_injection = |label: &'static str,
6416 default_state: &Vector4<f64>,
6417 expanded_state: &Vector4<f64>,
6418 xref: &[Vector4<f64>],
6419 warm_start: &[Vector2<f64>],
6420 default_row: &SwitchBackDetailedRow,
6421 expanded_row: &SwitchBackDetailedRow| {
6422 let injected_state = inject_state_components(default_state, expanded_state, v_indices);
6423 let result = iterative_linear_mpc_control_with_test_optimizer(
6424 xref,
6425 &injected_state,
6426 warm_start,
6427 optimize_linearized_controls_original,
6428 );
6429
6430 let default_baseline_controls: Vec<[f64; 2]> = default_row
6431 .controls
6432 .iter()
6433 .map(|vec| [vec[0], vec[1]])
6434 .collect();
6435 let expanded_baseline_controls: Vec<[f64; 2]> = expanded_row
6436 .controls
6437 .iter()
6438 .map(|vec| [vec[0], vec[1]])
6439 .collect();
6440 let default_baseline_predicted: Vec<[f64; 4]> = default_row
6441 .predicted
6442 .iter()
6443 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6444 .collect();
6445 let expanded_baseline_predicted: Vec<[f64; 4]> = expanded_row
6446 .predicted
6447 .iter()
6448 .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6449 .collect();
6450
6451 let to_default_control =
6452 summarize_vec2_sequence_diff(&result.controls, &default_baseline_controls);
6453 let to_expanded_control =
6454 summarize_vec2_sequence_diff(&result.controls, &expanded_baseline_controls);
6455 let to_default_predicted =
6456 summarize_vec4_sequence_diff(&result.predicted, &default_baseline_predicted);
6457 let to_expanded_predicted =
6458 summarize_vec4_sequence_diff(&result.predicted, &expanded_baseline_predicted);
6459
6460 ContextSwapInjectionResult {
6461 label,
6462 injected_steer: result.controls[0][1],
6463 default_baseline_steer: default_row.controls[0][1],
6464 expanded_baseline_steer: expanded_row.controls[0][1],
6465 default_same_control0: to_default_control[0],
6466 default_opposite_control0: to_expanded_control[0],
6467 default_same_pred_y: to_default_predicted[1],
6468 default_opposite_pred_y: to_expanded_predicted[1],
6469 default_same_pred_v: to_default_predicted[2],
6470 default_opposite_pred_v: to_expanded_predicted[2],
6471 }
6472 };
6473
6474 vec![
6475 run_injection(
6477 "baseline_325: d325_state + e325_v, d325_xref, ws325",
6478 &d325.state,
6479 &e325.state,
6480 &d325.xref,
6481 ws325,
6482 d325,
6483 e325,
6484 ),
6485 run_injection(
6487 "baseline_326: d326_state + e326_v, d326_xref, ws326",
6488 &d326.state,
6489 &e326.state,
6490 &d326.xref,
6491 ws326,
6492 d326,
6493 e326,
6494 ),
6495 run_injection(
6497 "swap_state: d325_state + e326_v, d326_xref, ws326",
6498 &d325.state,
6499 &e326.state,
6500 &d326.xref,
6501 ws326,
6502 d326,
6503 e326,
6504 ),
6505 run_injection(
6507 "swap_xref: d326_state + e326_v, d325_xref, ws326",
6508 &d326.state,
6509 &e326.state,
6510 &d325.xref,
6511 ws326,
6512 d326,
6513 e326,
6514 ),
6515 run_injection(
6517 "swap_ws: d326_state + e326_v, d326_xref, ws325",
6518 &d326.state,
6519 &e326.state,
6520 &d326.xref,
6521 ws325,
6522 d326,
6523 e326,
6524 ),
6525 run_injection(
6527 "swap_state+xref: d325_state + e326_v, d325_xref, ws326",
6528 &d325.state,
6529 &e326.state,
6530 &d325.xref,
6531 ws326,
6532 d326,
6533 e326,
6534 ),
6535 run_injection(
6537 "swap_state+ws: d325_state + e326_v, d326_xref, ws325",
6538 &d325.state,
6539 &e326.state,
6540 &d326.xref,
6541 ws325,
6542 d326,
6543 e326,
6544 ),
6545 run_injection(
6547 "swap_xref+ws: d326_state + e326_v, d325_xref, ws325",
6548 &d326.state,
6549 &e326.state,
6550 &d325.xref,
6551 ws325,
6552 d326,
6553 e326,
6554 ),
6555 run_injection(
6557 "swap_all: d325_state + e326_v, d325_xref, ws325",
6558 &d325.state,
6559 &e326.state,
6560 &d325.xref,
6561 ws325,
6562 d326,
6563 e326,
6564 ),
6565 run_injection(
6568 "cross_v_325in326: d326_state + e325_v, d326_xref, ws326",
6569 &d326.state,
6570 &e325.state,
6571 &d326.xref,
6572 ws326,
6573 d326,
6574 e326,
6575 ),
6576 run_injection(
6578 "cross_v_326in325: d325_state + e326_v, d325_xref, ws325",
6579 &d325.state,
6580 &e326.state,
6581 &d325.xref,
6582 ws325,
6583 d325,
6584 e325,
6585 ),
6586 {
6590 let mut s = d325.state;
6591 s[0] = d326.state[0]; run_injection(
6593 "isolate_x: d325{x→d326} + e325_v",
6594 &s,
6595 &e325.state,
6596 &d325.xref,
6597 ws325,
6598 d325,
6599 e325,
6600 )
6601 },
6602 {
6604 let mut s = d325.state;
6605 s[1] = d326.state[1]; run_injection(
6607 "isolate_y: d325{y→d326} + e325_v",
6608 &s,
6609 &e325.state,
6610 &d325.xref,
6611 ws325,
6612 d325,
6613 e325,
6614 )
6615 },
6616 {
6618 let mut s = d325.state;
6619 s[3] = d326.state[3]; run_injection(
6621 "isolate_yaw: d325{yaw→d326} + e325_v",
6622 &s,
6623 &e325.state,
6624 &d325.xref,
6625 ws325,
6626 d325,
6627 e325,
6628 )
6629 },
6630 {
6632 let mut s = d325.state;
6633 s[1] = d326.state[1];
6634 s[3] = d326.state[3];
6635 run_injection(
6636 "isolate_y+yaw: d325{y,yaw→d326} + e325_v",
6637 &s,
6638 &e325.state,
6639 &d325.xref,
6640 ws325,
6641 d325,
6642 e325,
6643 )
6644 },
6645 {
6647 let mut s = d325.state;
6648 s[0] = d326.state[0];
6649 s[1] = d326.state[1];
6650 run_injection(
6651 "isolate_x+y: d325{x,y→d326} + e325_v",
6652 &s,
6653 &e325.state,
6654 &d325.xref,
6655 ws325,
6656 d325,
6657 e325,
6658 )
6659 },
6660 {
6662 let mut s = d326.state;
6663 s[2] = d325.state[2]; run_injection(
6665 "isolate_all_nonv: d326{v→d325} + e325_v",
6666 &s,
6667 &e325.state,
6668 &d325.xref,
6669 ws325,
6670 d325,
6671 e325,
6672 )
6673 },
6674 ]
6675 }
6676
6677 #[test]
6678 #[ignore = "long-running regression scenario"]
6679 fn test_mpc_switch_back_characterizes_step_325_326_v_injection_context_swap() {
6680 let results = measure_step_325_326_context_swap_v_injection();
6681
6682 println!("=== Step 325 vs 326: V-Only Injection Context Swap ===");
6683 println!("Question: why does v-only opposite-branch injection work at 325 but not 326?");
6684 println!();
6685
6686 println!("=== Raw State Values ===");
6688 println!(" (indices: x=0, y=1, v=2, yaw=3)");
6689 let default_trace_print = collect_switch_back_detailed_trace_with_test_optimizer(
6691 330,
6692 optimize_linearized_controls_original,
6693 );
6694 let expanded_trace_print = collect_switch_back_detailed_trace_with_test_optimizer(
6695 330,
6696 optimize_linearized_controls_with_full_candidate_expansion,
6697 );
6698 let d325_p = detailed_row_at_step(&default_trace_print, 325);
6699 let d326_p = detailed_row_at_step(&default_trace_print, 326);
6700 let e325_p = detailed_row_at_step(&expanded_trace_print, 325);
6701 let e326_p = detailed_row_at_step(&expanded_trace_print, 326);
6702 println!(" d325.state: {:?}", d325_p.state);
6703 println!(" d326.state: {:?}", d326_p.state);
6704 println!(" d325→d326 delta: {:?}", d326_p.state - d325_p.state);
6705 println!(" e325.state: {:?}", e325_p.state);
6706 println!(" e326.state: {:?}", e326_p.state);
6707 println!(" e325→e326 delta: {:?}", e326_p.state - e325_p.state);
6708 println!(
6709 " v diff: e325.v={:.6} e326.v={:.6} delta={:.6}",
6710 e325_p.state[2],
6711 e326_p.state[2],
6712 e326_p.state[2] - e325_p.state[2]
6713 );
6714 println!();
6715
6716 for r in &results {
6717 let pull_ratio = if r.default_opposite_control0 > 0.0 {
6718 r.default_same_control0 / r.default_opposite_control0
6719 } else {
6720 f64::INFINITY
6721 };
6722 let pulls_toward_expanded = pull_ratio > 4.0;
6723 println!(
6724 "[{}] {} steer={:.6} (default={:.6}, expanded={:.6})",
6725 if pulls_toward_expanded {
6726 "PULL"
6727 } else {
6728 "STAY"
6729 },
6730 r.label,
6731 r.injected_steer,
6732 r.default_baseline_steer,
6733 r.expanded_baseline_steer,
6734 );
6735 println!(
6736 " same_ctrl0={:.4e} opp_ctrl0={:.4e} ratio={:.3} pred_y_s={:.4e} pred_y_o={:.4e} pred_v_s={:.4e} pred_v_o={:.4e}",
6737 r.default_same_control0,
6738 r.default_opposite_control0,
6739 pull_ratio,
6740 r.default_same_pred_y,
6741 r.default_opposite_pred_y,
6742 r.default_same_pred_v,
6743 r.default_opposite_pred_v,
6744 );
6745 }
6746
6747 let baseline_325 = &results[0];
6749 let ratio_325 = baseline_325.default_same_control0 / baseline_325.default_opposite_control0;
6750 assert!(
6751 ratio_325 > 100.0,
6752 "v-only injection should strongly pull toward expanded at step 325 (ratio={ratio_325}): {:?}",
6753 baseline_325
6754 );
6755
6756 let baseline_326 = &results[1];
6757 let ratio_326 = baseline_326.default_same_control0 / baseline_326.default_opposite_control0;
6758 assert!(
6759 ratio_326 < 10.0,
6760 "v-only injection pull should weaken dramatically at step 326 (ratio={ratio_326}): {:?}",
6761 baseline_326
6762 );
6763
6764 assert!(
6766 ratio_325 > 100.0 * ratio_326,
6767 "v-only pull should drop >100x from step 325 to 326: 325={ratio_325} 326={ratio_326}"
6768 );
6769
6770 let swap_xref = &results[3];
6772 let ratio_swap_xref = swap_xref.default_same_control0 / swap_xref.default_opposite_control0;
6773 assert!(
6774 (ratio_swap_xref - ratio_326).abs() < 0.1,
6775 "xref swap should not change ratio: baseline={ratio_326} swapped={ratio_swap_xref}"
6776 );
6777
6778 let swap_ws = &results[4];
6779 let ratio_swap_ws = swap_ws.default_same_control0 / swap_ws.default_opposite_control0;
6780 assert!(
6781 (ratio_swap_ws - ratio_326).abs() < 0.1,
6782 "warm_start swap should not change ratio: baseline={ratio_326} swapped={ratio_swap_ws}"
6783 );
6784
6785 let swap_state = &results[2];
6787 let ratio_swap_state =
6788 swap_state.default_same_control0 / swap_state.default_opposite_control0;
6789 assert!(
6790 (ratio_swap_state - ratio_326).abs() > 0.5,
6791 "state swap should change ratio: baseline={ratio_326} swapped={ratio_swap_state}"
6792 );
6793
6794 for result in &results[11..=16] {
6797 let ratio = result.default_same_control0 / result.default_opposite_control0;
6798 assert!(
6799 ratio > 100.0,
6800 "default non-v component change should not destroy pull: {} ratio={ratio}",
6801 result.label
6802 );
6803 }
6804 }
6805
6806 fn iterative_linear_mpc_control_with_max_iter(
6811 xref: &[Vector4<f64>],
6812 state: &State,
6813 warm_start: &[Vector2<f64>],
6814 max_iter: usize,
6815 ) -> MpcResult {
6816 let mut controls = warm_start.to_vec();
6817 controls.resize(T, Vector2::zeros());
6818 apply_control_constraints(&mut controls);
6819
6820 let mut predicted = predict_motion(*state, &controls);
6821 for _ in 0..max_iter {
6822 let previous_controls = controls.clone();
6823 controls = optimize_linearized_controls(xref, &predicted, state, &controls);
6824 predicted = predict_motion(*state, &controls);
6825
6826 let du = controls
6827 .iter()
6828 .zip(previous_controls.iter())
6829 .map(|(current, previous)| {
6830 (current[0] - previous[0]).abs() + (current[1] - previous[1]).abs()
6831 })
6832 .sum::<f64>();
6833
6834 if du <= DU_TH {
6835 break;
6836 }
6837 }
6838
6839 MpcResult {
6840 controls,
6841 predicted,
6842 }
6843 }
6844
6845 fn dump_first_inner_solve_adjoint(
6848 xref: &[Vector4<f64>],
6849 state: &State,
6850 warm_start: &[Vector2<f64>],
6851 ) -> (Vec<Vector2<f64>>, Vec<[f64; 5]>) {
6852 let mut controls = warm_start.to_vec();
6853 controls.resize(T, Vector2::zeros());
6854 apply_control_constraints(&mut controls);
6855
6856 let xbar = predict_motion(*state, &controls);
6857
6858 let q = state_cost_weight();
6859 let qf = terminal_cost_weight();
6860 let r = control_cost_weight();
6861 let rd = control_rate_weight();
6862
6863 let (x, a_seq, b_seq, _) = linearized_rollout(state, &xbar, &controls);
6864
6865 let mut gradients = vec![Vector2::zeros(); T];
6866 let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
6867 let mut lambda3_decomp = Vec::new();
6868
6869 lambda3_decomp.push([
6871 lambda[3], 0.0, 0.0, 0.0, lambda[3], ]);
6877
6878 for t in (0..T).rev() {
6879 let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
6880 if t > 0 {
6881 grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
6882 }
6883 if t < T - 1 {
6884 grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
6885 }
6886 gradients[t] = grad;
6887
6888 let lx = if t == 0 {
6889 Vector4::zeros()
6890 } else {
6891 (q * state_error(x[t], xref[t])) * 2.0
6892 };
6893
6894 let lambda_next = lambda;
6895 lambda = lx + a_seq[t].transpose() * lambda_next;
6896
6897 let from_lx3 = lx[3];
6899 let from_at30_l0 = a_seq[t][(0, 3)] * lambda_next[0]; let from_at31_l1 = a_seq[t][(1, 3)] * lambda_next[1]; let from_at33_l3 = a_seq[t][(3, 3)] * lambda_next[3]; lambda3_decomp.push([
6903 lambda[3],
6904 from_lx3,
6905 from_at30_l0,
6906 from_at31_l1,
6907 from_at33_l3,
6908 ]);
6909 }
6910
6911 lambda3_decomp.reverse();
6912 (gradients, lambda3_decomp)
6913 }
6914
6915 #[test]
6916 #[ignore = "long-running regression scenario"]
6917 fn test_mpc_switch_back_characterizes_y_effect_path_via_outer_loop() {
6918 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6919 330,
6920 optimize_linearized_controls_original,
6921 );
6922 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6923 330,
6924 optimize_linearized_controls_with_full_candidate_expansion,
6925 );
6926 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
6927 330,
6928 optimize_linearized_controls_original,
6929 );
6930
6931 println!("=== Y Effect Path: Outer Loop Iteration Count ===");
6932 println!("At step 326, v-only injection needs v+y. Is y's effect through outer-loop re-linearization?");
6933 println!();
6934
6935 for &step in &[325usize, 326, 327] {
6936 let d_row = detailed_row_at_step(&default_trace, step);
6937 let e_row = detailed_row_at_step(&expanded_trace, step);
6938 let ws = default_warm_start.get(&step).unwrap();
6939
6940 let d_baseline_steer = d_row.controls[0][1];
6941 let e_baseline_steer = e_row.controls[0][1];
6942
6943 println!("--- step {step} (default_steer={d_baseline_steer:.6}, expanded_steer={e_baseline_steer:.6}) ---");
6944
6945 for &max_iter in &[1usize, 2, 3] {
6946 let v_state = inject_state_components(&d_row.state, &e_row.state, &[2]);
6948 let v_result =
6949 iterative_linear_mpc_control_with_max_iter(&d_row.xref, &v_state, ws, max_iter);
6950
6951 let vy_state = inject_state_components(&d_row.state, &e_row.state, &[1, 2]);
6953 let vy_result = iterative_linear_mpc_control_with_max_iter(
6954 &d_row.xref,
6955 &vy_state,
6956 ws,
6957 max_iter,
6958 );
6959
6960 let v_steer = v_result.controls[0][1];
6961 let vy_steer = vy_result.controls[0][1];
6962 let steer_diff = (v_steer - vy_steer).abs();
6963
6964 println!(
6965 " iter={max_iter}: v_steer={v_steer:.6} vy_steer={vy_steer:.6} diff={steer_diff:.6e}"
6966 );
6967 }
6968 }
6969 }
6970
6971 fn linearized_rollout_without_angle_normalization(
6973 x0: &State,
6974 xbar: &[Vector4<f64>],
6975 controls: &[Vector2<f64>],
6976 ) -> LinearizedRollout {
6977 let mut x = vec![Vector4::zeros(); T + 1];
6978 let mut a_seq = Vec::with_capacity(T);
6979 let mut b_seq = Vec::with_capacity(T);
6980 let mut c_seq = Vec::with_capacity(T);
6981
6982 x[0] = x0.to_vector();
6983 for t in 0..T {
6984 let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
6985 let next = a * x[t] + b * controls[t] + c;
6986 a_seq.push(a);
6989 b_seq.push(b);
6990 c_seq.push(c);
6991 x[t + 1] = next;
6992 }
6993
6994 (x, a_seq, b_seq, c_seq)
6995 }
6996
6997 #[test]
6998 #[ignore = "long-running regression scenario"]
6999 fn test_mpc_switch_back_characterizes_yaw_normalize_effect_in_linearized_rollout() {
7000 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7001 330,
7002 optimize_linearized_controls,
7003 );
7004 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7005 330,
7006 optimize_linearized_controls,
7007 );
7008
7009 println!("=== Yaw Normalize Effect in Linearized Rollout ===");
7010 println!("Comparing linearized_rollout with and without normalize_angle");
7011 println!();
7012
7013 for &step in &[20usize, 70, 71, 75, 78, 325, 326] {
7014 let row = detailed_row_at_step(&default_trace, step);
7015 let ws = default_warm_start.get(&step).unwrap();
7016 let state = state_from_vector4(&row.state);
7017
7018 let mut controls = ws.to_vec();
7020 controls.resize(T, Vector2::zeros());
7021 apply_control_constraints(&mut controls);
7022 let xbar = predict_motion(state, &controls);
7023
7024 let (x_with, _, _, _) = linearized_rollout(&state, &xbar, &controls);
7025 let (x_without, _, _, _) =
7026 linearized_rollout_without_angle_normalization(&state, &xbar, &controls);
7027
7028 let mut max_yaw_diff = 0.0f64;
7029 let mut max_other_diff = [0.0f64; 3]; for t in 0..=T {
7031 let yaw_diff = (x_with[t][3] - x_without[t][3]).abs();
7032 max_yaw_diff = max_yaw_diff.max(yaw_diff);
7033 for (i, slot) in max_other_diff.iter_mut().enumerate() {
7034 *slot = (*slot).max((x_with[t][i] - x_without[t][i]).abs());
7035 }
7036 }
7037
7038 let yaw_values: Vec<f64> = (0..=T).map(|t| x_with[t][3]).collect();
7039 println!(
7040 "step {step:>3}: yaw_diff={max_yaw_diff:.4e} other=[{:.4e},{:.4e},{:.4e}] yaw={:.4?}",
7041 max_other_diff[0], max_other_diff[1], max_other_diff[2], yaw_values
7042 );
7043 }
7044 }
7045
7046 #[test]
7047 #[ignore = "long-running regression scenario"]
7048 fn test_mpc_switch_back_characterizes_adjoint_y_to_steer_coupling() {
7049 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7050 330,
7051 optimize_linearized_controls_original,
7052 );
7053 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7054 330,
7055 optimize_linearized_controls_with_full_candidate_expansion,
7056 );
7057 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7058 330,
7059 optimize_linearized_controls_original,
7060 );
7061
7062 println!("=== Adjoint Decomposition: How Y Reaches Steering ===");
7063 println!(
7064 "lambda3_decomp columns: [total, from_lx3, from_AT30*l0, from_AT31*l1, from_AT33*l3]"
7065 );
7066 println!();
7067
7068 for &step in &[325usize, 326] {
7069 let d_row = detailed_row_at_step(&default_trace, step);
7070 let e_row = detailed_row_at_step(&expanded_trace, step);
7071 let ws = default_warm_start.get(&step).unwrap();
7072
7073 println!("--- step {step} ---");
7074
7075 let (grad_base, l3_base) =
7077 dump_first_inner_solve_adjoint(&d_row.xref, &state_from_vector4(&d_row.state), ws);
7078 println!(" [baseline] steer_grad[0]={:.6e}", grad_base[0][1]);
7079 for (t, decomp) in l3_base.iter().enumerate() {
7080 println!(
7081 " t={t}: λ3={:.4e} lx3={:.4e} AT30·l0={:.4e} AT31·l1={:.4e} AT33·l3={:.4e}",
7082 decomp[0], decomp[1], decomp[2], decomp[3], decomp[4]
7083 );
7084 }
7085
7086 let v_state = inject_state_components(&d_row.state, &e_row.state, &[2]);
7088 let (grad_v, l3_v) = dump_first_inner_solve_adjoint(&d_row.xref, &v_state, ws);
7089 println!(" [v-only] steer_grad[0]={:.6e}", grad_v[0][1]);
7090 for (t, decomp) in l3_v.iter().enumerate() {
7091 println!(
7092 " t={t}: λ3={:.4e} lx3={:.4e} AT30·l0={:.4e} AT31·l1={:.4e} AT33·l3={:.4e}",
7093 decomp[0], decomp[1], decomp[2], decomp[3], decomp[4]
7094 );
7095 }
7096
7097 let vy_state = inject_state_components(&d_row.state, &e_row.state, &[1, 2]);
7099 let (grad_vy, l3_vy) = dump_first_inner_solve_adjoint(&d_row.xref, &vy_state, ws);
7100 println!(" [v+y] steer_grad[0]={:.6e}", grad_vy[0][1]);
7101 for (t, decomp) in l3_vy.iter().enumerate() {
7102 println!(
7103 " t={t}: λ3={:.4e} lx3={:.4e} AT30·l0={:.4e} AT31·l1={:.4e} AT33·l3={:.4e}",
7104 decomp[0], decomp[1], decomp[2], decomp[3], decomp[4]
7105 );
7106 }
7107
7108 println!(
7110 " steer_grad[0] diffs: v-base={:.6e} vy-base={:.6e} vy-v={:.6e}",
7111 grad_v[0][1] - grad_base[0][1],
7112 grad_vy[0][1] - grad_base[0][1],
7113 grad_vy[0][1] - grad_v[0][1],
7114 );
7115 println!();
7116 }
7117 }
7118
7119 #[test]
7123 #[ignore = "long-running regression scenario"]
7124 fn test_mpc_switch_back_characterizes_step_325_326_state_component_crossover() {
7125 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7126 330,
7127 optimize_linearized_controls_original,
7128 );
7129 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7130 330,
7131 optimize_linearized_controls_with_full_candidate_expansion,
7132 );
7133 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7134 330,
7135 optimize_linearized_controls_original,
7136 );
7137
7138 let d325 = detailed_row_at_step(&default_trace, 325);
7139 let d326 = detailed_row_at_step(&default_trace, 326);
7140 let e326 = detailed_row_at_step(&expanded_trace, 326);
7141 let ws326 = default_warm_start.get(&326).unwrap();
7142
7143 let state_delta = d326.state - d325.state;
7144 println!(
7145 "state_delta: x={:.6e} y={:.6e} v={:.6e} yaw={:.6e}",
7146 state_delta[0], state_delta[1], state_delta[2], state_delta[3]
7147 );
7148
7149 let expanded_baseline_controls: Vec<[f64; 2]> =
7150 e326.controls.iter().map(|v| [v[0], v[1]]).collect();
7151 let default_baseline_controls: Vec<[f64; 2]> =
7152 d326.controls.iter().map(|v| [v[0], v[1]]).collect();
7153
7154 let component_sets: Vec<(&str, Vec<usize>)> = vec![
7155 ("revert_x", vec![0]),
7156 ("revert_y", vec![1]),
7157 ("revert_v", vec![2]),
7158 ("revert_yaw", vec![3]),
7159 ("revert_y+v", vec![1, 2]),
7160 ("revert_x+yaw", vec![0, 3]),
7161 ("revert_all", vec![0, 1, 2, 3]),
7162 ];
7163
7164 println!(
7165 "\n=== State Component Crossover (step 326 context, revert components to 325) ==="
7166 );
7167
7168 let baseline_injected_state = inject_state_components(&d326.state, &e326.state, &[2]);
7170 let baseline_result = iterative_linear_mpc_control_with_test_optimizer(
7171 &d326.xref,
7172 &baseline_injected_state,
7173 ws326,
7174 optimize_linearized_controls_original,
7175 );
7176 let baseline_to_default =
7177 summarize_vec2_sequence_diff(&baseline_result.controls, &default_baseline_controls);
7178 let baseline_to_expanded =
7179 summarize_vec2_sequence_diff(&baseline_result.controls, &expanded_baseline_controls);
7180 let baseline_ratio = if baseline_to_expanded[0] > 0.0 {
7181 baseline_to_default[0] / baseline_to_expanded[0]
7182 } else {
7183 f64::INFINITY
7184 };
7185 println!(
7186 " baseline: same={:.4e} opp={:.4e} ratio={:.3}",
7187 baseline_to_default[0], baseline_to_expanded[0], baseline_ratio
7188 );
7189
7190 let mut ratios = Vec::new();
7191 for (label, indices) in &component_sets {
7192 let mut modified_default = d326.state;
7193 for &idx in indices {
7194 modified_default[idx] = d325.state[idx];
7195 }
7196
7197 let injected_state = inject_state_components(&modified_default, &e326.state, &[2]);
7198 let result = iterative_linear_mpc_control_with_test_optimizer(
7199 &d326.xref,
7200 &injected_state,
7201 ws326,
7202 optimize_linearized_controls_original,
7203 );
7204
7205 let to_default =
7206 summarize_vec2_sequence_diff(&result.controls, &default_baseline_controls);
7207 let to_expanded =
7208 summarize_vec2_sequence_diff(&result.controls, &expanded_baseline_controls);
7209 let ratio = if to_expanded[0] > 0.0 {
7210 to_default[0] / to_expanded[0]
7211 } else {
7212 f64::INFINITY
7213 };
7214
7215 println!(
7216 " {:12} same={:.4e} opp={:.4e} ratio={:.3} (delta: {:.3})",
7217 label,
7218 to_default[0],
7219 to_expanded[0],
7220 ratio,
7221 ratio - baseline_ratio
7222 );
7223 ratios.push((*label, ratio));
7224 }
7225
7226 let revert_y_ratio = ratios.iter().find(|(l, _)| *l == "revert_y").unwrap().1;
7227 let revert_v_ratio = ratios.iter().find(|(l, _)| *l == "revert_v").unwrap().1;
7228 let revert_yv_ratio = ratios.iter().find(|(l, _)| *l == "revert_y+v").unwrap().1;
7229 let revert_all_ratio = ratios.iter().find(|(l, _)| *l == "revert_all").unwrap().1;
7230
7231 println!("\n=== Summary ===");
7232 println!("baseline_326 ratio: {:.3}", baseline_ratio);
7233 println!("revert_y ratio: {:.3}", revert_y_ratio);
7234 println!("revert_v ratio: {:.3}", revert_v_ratio);
7235 println!("revert_y+v ratio: {:.3}", revert_yv_ratio);
7236 println!("revert_all ratio: {:.3}", revert_all_ratio);
7237
7238 assert!(
7240 (revert_y_ratio - revert_all_ratio).abs() < 0.1,
7241 "reverting y alone should account for nearly all the effect: y={revert_y_ratio} all={revert_all_ratio}"
7242 );
7243
7244 assert!(
7246 (revert_v_ratio - baseline_ratio).abs() < 0.01,
7247 "reverting v should have no effect: v={revert_v_ratio} baseline={baseline_ratio}"
7248 );
7249
7250 let revert_xyaw_ratio = ratios.iter().find(|(l, _)| *l == "revert_x+yaw").unwrap().1;
7252 assert!(
7253 (revert_xyaw_ratio - baseline_ratio).abs() < 0.01,
7254 "reverting x+yaw should have no effect: x+yaw={revert_xyaw_ratio} baseline={baseline_ratio}"
7255 );
7256 }
7257
7258 #[test]
7261 #[ignore = "long-running regression scenario"]
7262 fn test_mpc_switch_back_characterizes_y_divergence_history() {
7263 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7264 350,
7265 optimize_linearized_controls_original,
7266 );
7267 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7268 350,
7269 optimize_linearized_controls_with_full_candidate_expansion,
7270 );
7271
7272 println!("=== state.y Divergence History (default vs expanded) ===");
7273 println!(
7274 "{:>5} {:>12} {:>12} {:>12} {:>12} {:>12}",
7275 "step", "default_y", "expanded_y", "y_diff", "default_v", "v_diff"
7276 );
7277
7278 let mut first_significant_step = None;
7279 let mut max_y_diff: f64 = 0.0;
7280
7281 for step in (0..350).step_by(5) {
7282 let d = default_trace.iter().find(|r| r.step == step);
7283 let e = expanded_trace.iter().find(|r| r.step == step);
7284 if let (Some(d), Some(e)) = (d, e) {
7285 let y_diff = (d.state[1] - e.state[1]).abs();
7286 max_y_diff = max_y_diff.max(y_diff);
7287
7288 if y_diff > 1e-6 || step >= 60 {
7289 println!(
7290 "{:5} {:12.6} {:12.6} {:12.6e} {:12.6} {:12.6e}",
7291 step,
7292 d.state[1],
7293 e.state[1],
7294 d.state[1] - e.state[1],
7295 d.state[2],
7296 d.state[2] - e.state[2]
7297 );
7298 }
7299
7300 if first_significant_step.is_none() && y_diff > 1e-3 {
7301 first_significant_step = Some(step);
7302 }
7303 }
7304 }
7305
7306 if let Some(start) = first_significant_step {
7308 let dense_start = start.saturating_sub(10);
7309 println!(
7310 "\n=== Dense view around first significant y divergence (step {}) ===",
7311 start
7312 );
7313 for step in dense_start..=(start + 20).min(349) {
7314 let d = default_trace.iter().find(|r| r.step == step);
7315 let e = expanded_trace.iter().find(|r| r.step == step);
7316 if let (Some(d), Some(e)) = (d, e) {
7317 let y_diff = d.state[1] - e.state[1];
7318 let v_diff = d.state[2] - e.state[2];
7319 let steer_diff = d.controls[0][1] - e.controls[0][1];
7320 println!(
7321 " step {:3}: y_diff={:12.6e} v_diff={:12.6e} steer_diff={:12.6e}",
7322 step, y_diff, v_diff, steer_diff
7323 );
7324 }
7325 }
7326 }
7327
7328 println!("\n=== Dense view step 315..335 (pre-terminal) ===");
7330 for step in 315..=335 {
7331 let d = default_trace.iter().find(|r| r.step == step);
7332 let e = expanded_trace.iter().find(|r| r.step == step);
7333 if let (Some(d), Some(e)) = (d, e) {
7334 let y_diff = d.state[1] - e.state[1];
7335 let v_diff = d.state[2] - e.state[2];
7336 let steer_diff = d.controls[0][1] - e.controls[0][1];
7337 println!(
7338 " step {:3}: y={:10.6}/{:10.6} diff={:12.6e} v_diff={:12.6e} steer_diff={:12.6e}",
7339 step, d.state[1], e.state[1], y_diff, v_diff, steer_diff
7340 );
7341 }
7342 }
7343
7344 println!("\nmax |y_diff| across all steps: {:.6e}", max_y_diff);
7345 if let Some(s) = first_significant_step {
7346 println!("first step with |y_diff| > 1e-3: {}", s);
7347 }
7348 }
7349
7350 fn optimize_linearized_controls_without_angle_wrap(
7353 xref: &[Vector4<f64>],
7354 xbar: &[Vector4<f64>],
7355 state: &State,
7356 initial_controls: &[Vector2<f64>],
7357 ) -> Vec<Vector2<f64>> {
7358 let (controls, _) = optimize_linearized_controls_without_angle_wrap_with_trace(
7359 xref,
7360 xbar,
7361 state,
7362 initial_controls,
7363 QP_MAX_ITERS,
7364 LINE_SEARCH_ITERS,
7365 );
7366 controls
7367 }
7368
7369 #[test]
7370 #[ignore = "long-running regression scenario"]
7371 fn test_mpc_switch_back_no_angle_wrap_closed_loop_parity() {
7372 let python_final_state = [
7374 -0.014_888_855_512_389,
7375 0.698_023_990_780_443,
7376 0.132_179_560_675_735,
7377 1.589_745_981_585_03,
7378 ];
7379 let python_history_length = 347usize;
7380
7381 let default_result = run_mpc_simulation();
7383 let default_final = default_result.final_state.to_vector();
7384 let default_len = default_result.hist_x.len();
7385
7386 let (cx, cy, cyaw) = generate_switch_back_course();
7388 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
7389
7390 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
7391 if state.yaw - cyaw[0] >= PI {
7392 state.yaw -= 2.0 * PI;
7393 } else if state.yaw - cyaw[0] <= -PI {
7394 state.yaw += 2.0 * PI;
7395 }
7396
7397 let final_index = cx.len() - 1;
7398 let goal = (cx[final_index], cy[final_index]);
7399 let mut target_index = 0;
7400 let mut warm_start = vec![Vector2::zeros(); T];
7401 let mut step_count = 0;
7402
7403 for _ in 0..MAX_SIM_STEPS {
7404 let (xref, new_index) = calc_ref_trajectory(
7405 &state,
7406 &cx,
7407 &cy,
7408 &cyaw,
7409 &speed_profile,
7410 UPSTREAM_SWITCH_BACK_TICK,
7411 target_index,
7412 );
7413 target_index = new_index;
7414
7415 let result = iterative_linear_mpc_control_with_test_optimizer(
7416 &xref,
7417 &state,
7418 &warm_start,
7419 optimize_linearized_controls_without_angle_wrap,
7420 );
7421
7422 state.update(result.controls[0][0], result.controls[0][1]);
7423 warm_start = next_warm_start(&result.controls);
7424 step_count += 1;
7425
7426 if check_goal(&state, goal, target_index, final_index) {
7427 break;
7428 }
7429 }
7430
7431 let nowrap_final = state.to_vector();
7432 let nowrap_len = step_count + 1; let default_diffs: Vec<f64> = (0..4)
7436 .map(|i| (default_final[i] - python_final_state[i]).abs())
7437 .collect();
7438 let nowrap_diffs: Vec<f64> = (0..4)
7439 .map(|i| (nowrap_final[i] - python_final_state[i]).abs())
7440 .collect();
7441
7442 println!("=== Closed-Loop Parity: Default vs No-Angle-Wrap ===");
7443 println!(
7444 "Python reference: {:?} (len={})",
7445 python_final_state, python_history_length
7446 );
7447 println!();
7448 println!("Default (with normalize_angle):");
7449 println!(
7450 " final: [{:.6}, {:.6}, {:.6}, {:.6}] (len={})",
7451 default_final[0], default_final[1], default_final[2], default_final[3], default_len
7452 );
7453 println!(
7454 " diffs: [{:.6e}, {:.6e}, {:.6e}, {:.6e}] step_diff={}",
7455 default_diffs[0],
7456 default_diffs[1],
7457 default_diffs[2],
7458 default_diffs[3],
7459 (default_len as isize - python_history_length as isize).abs()
7460 );
7461 println!();
7462 println!("No-angle-wrap:");
7463 println!(
7464 " final: [{:.6}, {:.6}, {:.6}, {:.6}] (len={})",
7465 nowrap_final[0], nowrap_final[1], nowrap_final[2], nowrap_final[3], nowrap_len
7466 );
7467 println!(
7468 " diffs: [{:.6e}, {:.6e}, {:.6e}, {:.6e}] step_diff={}",
7469 nowrap_diffs[0],
7470 nowrap_diffs[1],
7471 nowrap_diffs[2],
7472 nowrap_diffs[3],
7473 (nowrap_len as isize - python_history_length as isize).abs()
7474 );
7475 println!();
7476
7477 let default_total: f64 = default_diffs.iter().sum();
7479 let nowrap_total: f64 = nowrap_diffs.iter().sum();
7480 println!(
7481 "Total state diff: default={:.6e} nowrap={:.6e} improvement={:.2}%",
7482 default_total,
7483 nowrap_total,
7484 if default_total > 0.0 {
7485 (1.0 - nowrap_total / default_total) * 100.0
7486 } else {
7487 0.0
7488 }
7489 );
7490 }
7491
7492 #[allow(clippy::needless_range_loop, clippy::type_complexity)]
7509 fn solve_mpc_qp_clarabel(
7510 xref: &[Vector4<f64>],
7511 xbar: &[Vector4<f64>],
7512 state: &State,
7513 ) -> Option<(Vec<Vector2<f64>>, Vec<Vector4<f64>>)> {
7514 use clarabel::algebra::*;
7515 use clarabel::solver::*;
7516
7517 let nu = 2; let nx = 4; let n_u = nu * T; let n_x = nx * (T + 1); let n = n_u + n_x; let u_idx = |t: usize, k: usize| -> usize { t * nu + k };
7525 let x_idx = |t: usize, k: usize| -> usize { n_u + t * nx + k };
7526
7527 let r_diag = R;
7529 let rd_diag = RD;
7530 let q_diag = Q;
7531 let qf_diag = QF;
7532
7533 let mut p_rows = Vec::new();
7534 let mut p_cols = Vec::new();
7535 let mut p_vals = Vec::new();
7536
7537 for t in 0..T {
7539 for k in 0..nu {
7540 let idx = u_idx(t, k);
7541 let mut val = r_diag[k];
7542 if t > 0 {
7544 val += rd_diag[k];
7545 }
7546 if t < T - 1 {
7547 val += rd_diag[k];
7548 }
7549 p_rows.push(idx);
7550 p_cols.push(idx);
7551 p_vals.push(val * 2.0); }
7553 if t < T - 1 {
7555 for k in 0..nu {
7556 let i = u_idx(t, k);
7557 let j = u_idx(t + 1, k);
7558 p_rows.push(i);
7560 p_cols.push(j);
7561 p_vals.push(-rd_diag[k] * 2.0);
7562 }
7563 }
7564 }
7565
7566 for t in 1..=T {
7568 let w = if t == T { qf_diag } else { q_diag };
7569 for k in 0..nx {
7570 let idx = x_idx(t, k);
7571 p_rows.push(idx);
7572 p_cols.push(idx);
7573 p_vals.push(w[k] * 2.0);
7574 }
7575 }
7576
7577 let p = CscMatrix::new_from_triplets(n, n, p_rows.clone(), p_cols.clone(), p_vals.clone());
7578
7579 let mut q_vec = vec![0.0; n];
7581 for t in 1..=T {
7582 let w = if t == T { qf_diag } else { q_diag };
7583 for k in 0..nx {
7584 q_vec[x_idx(t, k)] = -w[k] * 2.0 * xref[t][k];
7587 }
7588 }
7589
7590 let n_eq = nx * (T + 1); let n_steer_rate = T - 1; let n_u_box = 2 * nu * T; let n_steer_rate_box = 2 * n_steer_rate; let n_v_box = 2 * (T + 1); let n_ineq = n_u_box + n_steer_rate_box + n_v_box;
7601
7602 let mut a_rows = Vec::new();
7603 let mut a_cols = Vec::new();
7604 let mut a_vals = Vec::new();
7605 let mut b_vec = vec![0.0; n_eq + n_ineq];
7606
7607 let x0 = state.to_vector();
7609 for k in 0..nx {
7610 let row = k;
7611 a_rows.push(row);
7612 a_cols.push(x_idx(0, k));
7613 a_vals.push(1.0);
7614 b_vec[row] = x0[k];
7615 }
7616
7617 for t in 0..T {
7619 let (a_mat, b_mat, c_vec) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
7620 let base_row = nx * (t + 1);
7621
7622 for i in 0..nx {
7625 let row = base_row + i;
7626 a_rows.push(row);
7628 a_cols.push(x_idx(t + 1, i));
7629 a_vals.push(-1.0);
7630 for j in 0..nx {
7632 let val = a_mat[(i, j)];
7633 if val.abs() > 1e-15 {
7634 a_rows.push(row);
7635 a_cols.push(x_idx(t, j));
7636 a_vals.push(val);
7637 }
7638 }
7639 for j in 0..nu {
7641 let val = b_mat[(i, j)];
7642 if val.abs() > 1e-15 {
7643 a_rows.push(row);
7644 a_cols.push(u_idx(t, j));
7645 a_vals.push(val);
7646 }
7647 }
7648 b_vec[row] = -c_vec[i];
7649 }
7650 }
7651
7652 let ineq_offset = n_eq;
7655 let mut ineq_row = 0;
7656
7657 let u_bounds = [MAX_ACCEL, MAX_STEER];
7659 for t in 0..T {
7660 for (k, &bound) in u_bounds.iter().enumerate() {
7661 let row = ineq_offset + ineq_row;
7663 a_rows.push(row);
7664 a_cols.push(u_idx(t, k));
7665 a_vals.push(1.0);
7666 b_vec[row] = bound;
7667 ineq_row += 1;
7668
7669 let row = ineq_offset + ineq_row;
7671 a_rows.push(row);
7672 a_cols.push(u_idx(t, k));
7673 a_vals.push(-1.0);
7674 b_vec[row] = bound;
7675 ineq_row += 1;
7676 }
7677 }
7678
7679 let max_steer_delta = MAX_DSTEER * DT;
7681 for t in 0..(T - 1) {
7682 let row = ineq_offset + ineq_row;
7684 a_rows.push(row);
7685 a_cols.push(u_idx(t + 1, 1));
7686 a_vals.push(1.0);
7687 a_rows.push(row);
7688 a_cols.push(u_idx(t, 1));
7689 a_vals.push(-1.0);
7690 b_vec[row] = max_steer_delta;
7691 ineq_row += 1;
7692
7693 let row = ineq_offset + ineq_row;
7695 a_rows.push(row);
7696 a_cols.push(u_idx(t + 1, 1));
7697 a_vals.push(-1.0);
7698 a_rows.push(row);
7699 a_cols.push(u_idx(t, 1));
7700 a_vals.push(1.0);
7701 b_vec[row] = max_steer_delta;
7702 ineq_row += 1;
7703 }
7704
7705 for t in 0..=T {
7707 let row = ineq_offset + ineq_row;
7709 a_rows.push(row);
7710 a_cols.push(x_idx(t, 2));
7711 a_vals.push(1.0);
7712 b_vec[row] = MAX_SPEED;
7713 ineq_row += 1;
7714
7715 let row = ineq_offset + ineq_row;
7717 a_rows.push(row);
7718 a_cols.push(x_idx(t, 2));
7719 a_vals.push(-1.0);
7720 b_vec[row] = -MIN_SPEED;
7721 ineq_row += 1;
7722 }
7723
7724 let m = n_eq + n_ineq;
7725
7726 let a_csc = CscMatrix::new_from_triplets(m, n, a_rows, a_cols, a_vals);
7729
7730 let cones = vec![
7732 SupportedConeT::ZeroConeT(n_eq), SupportedConeT::NonnegativeConeT(n_ineq), ];
7735
7736 let settings = DefaultSettingsBuilder::default()
7737 .verbose(false)
7738 .build()
7739 .unwrap();
7740
7741 let mut solver = DefaultSolver::new(&p, &q_vec, &a_csc, &b_vec, &cones, settings).ok()?;
7742 solver.solve();
7743
7744 if solver.solution.status != SolverStatus::Solved {
7745 return None;
7746 }
7747
7748 let z = &solver.solution.x;
7749
7750 let controls: Vec<Vector2<f64>> = (0..T)
7751 .map(|t| Vector2::new(z[u_idx(t, 0)], z[u_idx(t, 1)]))
7752 .collect();
7753
7754 let predicted: Vec<Vector4<f64>> = (0..=T)
7755 .map(|t| {
7756 Vector4::new(
7757 z[x_idx(t, 0)],
7758 z[x_idx(t, 1)],
7759 z[x_idx(t, 2)],
7760 z[x_idx(t, 3)],
7761 )
7762 })
7763 .collect();
7764
7765 Some((controls, predicted))
7766 }
7767
7768 fn optimize_linearized_controls_qp(
7770 xref: &[Vector4<f64>],
7771 xbar: &[Vector4<f64>],
7772 state: &State,
7773 _initial_controls: &[Vector2<f64>],
7774 ) -> Vec<Vector2<f64>> {
7775 if let Some((controls, _)) = solve_mpc_qp_clarabel(xref, xbar, state) {
7776 controls
7777 } else {
7778 optimize_linearized_controls(xref, xbar, state, _initial_controls)
7780 }
7781 }
7782
7783 #[test]
7784 #[ignore = "long-running regression scenario"]
7785 fn test_mpc_qp_solver_matches_projected_gradient_on_representative_steps() {
7786 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7787 80,
7788 optimize_linearized_controls,
7789 );
7790 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7791 80,
7792 optimize_linearized_controls,
7793 );
7794
7795 println!("=== QP Solver (clarabel) vs Projected-Gradient ===");
7796 println!();
7797
7798 for &step in &[20usize, 70, 75, 78] {
7799 let row = detailed_row_at_step(&default_trace, step);
7800 let ws = default_warm_start.get(&step).unwrap();
7801 let state = state_from_vector4(&row.state);
7802
7803 let mut controls_for_xbar = ws.to_vec();
7805 controls_for_xbar.resize(T, Vector2::zeros());
7806 apply_control_constraints(&mut controls_for_xbar);
7807 let xbar = predict_motion(state, &controls_for_xbar);
7808
7809 let pg_controls = optimize_linearized_controls(&row.xref, &xbar, &state, ws);
7811
7812 let qp_result = solve_mpc_qp_clarabel(&row.xref, &xbar, &state);
7814
7815 if let Some((qp_controls, _qp_predicted)) = qp_result {
7816 let control_diff = summarize_vec2_sequence_diff(
7817 &qp_controls,
7818 &pg_controls.iter().map(|c| [c[0], c[1]]).collect::<Vec<_>>(),
7819 );
7820 println!(
7821 "step {step}: control_diff=[{:.6e}, {:.6e}] pg_steer={:.6} qp_steer={:.6}",
7822 control_diff[0], control_diff[1], pg_controls[0][1], qp_controls[0][1],
7823 );
7824 } else {
7825 println!("step {step}: QP solver failed!");
7826 }
7827 }
7828 }
7829
7830 #[test]
7831 #[ignore = "long-running regression scenario"]
7832 fn test_mpc_qp_solver_closed_loop_parity() {
7833 let python_final_state = [
7834 -0.014_888_855_512_389,
7835 0.698_023_990_780_443,
7836 0.132_179_560_675_735,
7837 1.589_745_981_585_03,
7838 ];
7839 let python_history_length = 347usize;
7840
7841 let default_result = run_mpc_simulation();
7842 let default_final = default_result.final_state.to_vector();
7843
7844 let (cx, cy, cyaw) = generate_switch_back_course();
7846 let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
7847
7848 let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
7849 if state.yaw - cyaw[0] >= PI {
7850 state.yaw -= 2.0 * PI;
7851 } else if state.yaw - cyaw[0] <= -PI {
7852 state.yaw += 2.0 * PI;
7853 }
7854
7855 let final_index = cx.len() - 1;
7856 let goal = (cx[final_index], cy[final_index]);
7857 let mut target_index = 0;
7858 let mut warm_start = vec![Vector2::zeros(); T];
7859 let mut step_count = 0;
7860
7861 for _ in 0..MAX_SIM_STEPS {
7862 let (xref, new_index) = calc_ref_trajectory(
7863 &state,
7864 &cx,
7865 &cy,
7866 &cyaw,
7867 &speed_profile,
7868 UPSTREAM_SWITCH_BACK_TICK,
7869 target_index,
7870 );
7871 target_index = new_index;
7872
7873 let result = iterative_linear_mpc_control_with_test_optimizer(
7874 &xref,
7875 &state,
7876 &warm_start,
7877 optimize_linearized_controls_qp,
7878 );
7879
7880 state.update(result.controls[0][0], result.controls[0][1]);
7881 warm_start = next_warm_start(&result.controls);
7882 step_count += 1;
7883
7884 if check_goal(&state, goal, target_index, final_index) {
7885 break;
7886 }
7887 }
7888
7889 let qp_final = state.to_vector();
7890 let qp_len = step_count + 1;
7891
7892 let default_diffs: Vec<f64> = (0..4)
7893 .map(|i| (default_final[i] - python_final_state[i]).abs())
7894 .collect();
7895 let qp_diffs: Vec<f64> = (0..4)
7896 .map(|i| (qp_final[i] - python_final_state[i]).abs())
7897 .collect();
7898
7899 println!("=== Closed-Loop Parity: PG vs QP (clarabel) ===");
7900 println!(
7901 "Python ref: {:?} (len={})",
7902 python_final_state, python_history_length
7903 );
7904 println!();
7905 println!(
7906 "PG: [{:.6},{:.6},{:.6},{:.6}] len={} diffs=[{:.4e},{:.4e},{:.4e},{:.4e}]",
7907 default_final[0],
7908 default_final[1],
7909 default_final[2],
7910 default_final[3],
7911 default_result.hist_x.len(),
7912 default_diffs[0],
7913 default_diffs[1],
7914 default_diffs[2],
7915 default_diffs[3]
7916 );
7917 println!(
7918 "QP: [{:.6},{:.6},{:.6},{:.6}] len={} diffs=[{:.4e},{:.4e},{:.4e},{:.4e}]",
7919 qp_final[0],
7920 qp_final[1],
7921 qp_final[2],
7922 qp_final[3],
7923 qp_len,
7924 qp_diffs[0],
7925 qp_diffs[1],
7926 qp_diffs[2],
7927 qp_diffs[3]
7928 );
7929
7930 let pg_total: f64 = default_diffs.iter().sum();
7931 let qp_total: f64 = qp_diffs.iter().sum();
7932 println!(
7933 "Total: PG={:.4e} QP={:.4e} improvement={:.1}%",
7934 pg_total,
7935 qp_total,
7936 if pg_total > 0.0 {
7937 (1.0 - qp_total / pg_total) * 100.0
7938 } else {
7939 0.0
7940 }
7941 );
7942 }
7943
7944 #[test]
7945 #[ignore = "long-running regression scenario"]
7946 fn test_mpc_solver_comparison_qp_vs_pg_detailed() {
7947 let expected_trace =
7948 parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
7949
7950 let hybrid_trace = collect_switch_back_detailed_trace(350); let qp_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7953 350,
7954 optimize_linearized_controls_qp,
7955 );
7956 let pg_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7957 350,
7958 optimize_linearized_controls_with_full_candidate_expansion,
7959 );
7960 let pg_orig_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7961 350,
7962 optimize_linearized_controls_original,
7963 );
7964
7965 println!("=== Solver Comparison (state diff sum vs Python) ===");
7966 println!(
7967 "{:>5} {:>10} {:>10} {:>10} {:>10} best",
7968 "step", "hybrid", "qp-only", "pg-exp", "pg-orig"
7969 );
7970
7971 let check_steps: Vec<usize> = (0..=80)
7972 .step_by(5)
7973 .chain([71, 72, 73, 74, 75, 76, 77, 78, 79, 80])
7974 .chain((85..350).step_by(10))
7975 .collect();
7976
7977 for &step in &check_steps {
7978 if step >= expected_trace.len() {
7979 continue;
7980 }
7981 let py = &expected_trace[step];
7982 let hy = hybrid_trace.iter().find(|r| r.step == step);
7983 let qp = qp_trace.iter().find(|r| r.step == step);
7984 let pg = pg_trace.iter().find(|r| r.step == step);
7985 let po = pg_orig_trace.iter().find(|r| r.step == step);
7986
7987 if let (Some(hy), Some(qp), Some(pg), Some(po)) = (hy, qp, pg, po) {
7988 let hy_s: f64 = trace_state_diff(&hy.state, &py.state).iter().sum();
7989 let qp_s: f64 = trace_state_diff(&qp.state, &py.state).iter().sum();
7990 let pg_s: f64 = trace_state_diff(&pg.state, &py.state).iter().sum();
7991 let po_s: f64 = trace_state_diff(&po.state, &py.state).iter().sum();
7992
7993 let min_s = hy_s.min(qp_s).min(pg_s).min(po_s);
7994 let best = if (hy_s - min_s).abs() < 1e-15 {
7995 "HYBRID"
7996 } else if (qp_s - min_s).abs() < 1e-15 {
7997 "QP"
7998 } else if (pg_s - min_s).abs() < 1e-15 {
7999 "PG-exp"
8000 } else {
8001 "PG-orig"
8002 };
8003
8004 println!(
8005 "{:5} {:10.4e} {:10.4e} {:10.4e} {:10.4e} {}",
8006 step, hy_s, qp_s, pg_s, po_s, best
8007 );
8008 }
8009 }
8010
8011 let mut wins = [0u32; 4]; let labels = ["HYBRID", "QP-only", "PG-expanded", "PG-original"];
8014 let min_steps = [
8015 hybrid_trace.len(),
8016 qp_trace.len(),
8017 pg_trace.len(),
8018 pg_orig_trace.len(),
8019 expected_trace.len(),
8020 ]
8021 .into_iter()
8022 .min()
8023 .unwrap();
8024 for (step, py) in expected_trace.iter().enumerate().take(min_steps) {
8025 let traces = [&hybrid_trace, &qp_trace, &pg_trace, &pg_orig_trace];
8026 let diffs: Vec<f64> = traces
8027 .iter()
8028 .map(|t| {
8029 t.iter()
8030 .find(|r| r.step == step)
8031 .map(|r| trace_state_diff(&r.state, &py.state).iter().sum())
8032 .unwrap_or(f64::INFINITY)
8033 })
8034 .collect();
8035 let min_d = diffs.iter().copied().fold(f64::INFINITY, f64::min);
8036 for (i, &d) in diffs.iter().enumerate() {
8037 if (d - min_d).abs() < 1e-15 {
8038 wins[i] += 1;
8039 break;
8040 }
8041 }
8042 }
8043 println!("\n=== Win count (closest to Python per step, out of {min_steps}) ===");
8044 for (i, label) in labels.iter().enumerate() {
8045 println!("{:12}: {} steps", label, wins[i]);
8046 }
8047 }
8048
8049 #[test]
8050 #[ignore = "long-running regression scenario"]
8051 fn test_mpc_switch_back_characterizes_v_injection_pull_ratio_threshold() {
8052 let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
8053 330,
8054 optimize_linearized_controls,
8055 );
8056 let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
8057 330,
8058 optimize_linearized_controls_with_full_candidate_expansion,
8059 );
8060 let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
8061 330,
8062 optimize_linearized_controls,
8063 );
8064
8065 let d325 = detailed_row_at_step(&default_trace, 325);
8066 let e325 = detailed_row_at_step(&expanded_trace, 325);
8067 let e326 = detailed_row_at_step(&expanded_trace, 326);
8068 let ws = default_warm_start.get(&325).unwrap();
8069
8070 let d_v = d325.state[2]; let e325_v = e325.state[2]; let e326_v = e326.state[2]; let d_baseline_controls: Vec<[f64; 2]> =
8076 d325.controls.iter().map(|v| [v[0], v[1]]).collect();
8077 let e_baseline_controls: Vec<[f64; 2]> =
8078 e325.controls.iter().map(|v| [v[0], v[1]]).collect();
8079
8080 println!("=== V-Injection Pull Ratio Threshold Sweep ===");
8081 println!(
8082 "default v={:.6}, e325_v={:.6}, e326_v={:.6}",
8083 d_v, e325_v, e326_v
8084 );
8085 println!("Sweeping injected v from e325_v to e326_v in 0.02 steps");
8086 println!();
8087 println!(
8088 "{:>10} {:>10} {:>10} {:>10} {:>10}",
8089 "inj_v", "steer", "same_c0", "opp_c0", "ratio"
8090 );
8091
8092 let n_steps = 11;
8094 let mut prev_ratio = f64::INFINITY;
8095 let mut threshold_v = None;
8096
8097 for i in 0..=n_steps {
8098 let frac = i as f64 / n_steps as f64;
8099 let injected_v = e325_v + frac * (e326_v - e325_v);
8100
8101 let mut hybrid = d325.state;
8103 hybrid[2] = injected_v;
8104 let hybrid_state = state_from_vector4(&hybrid);
8105
8106 let result = iterative_linear_mpc_control_with_test_optimizer(
8107 &d325.xref,
8108 &hybrid_state,
8109 ws,
8110 optimize_linearized_controls,
8111 );
8112
8113 let same_c0 = summarize_vec2_sequence_diff(&result.controls, &d_baseline_controls)[0];
8114 let opp_c0 = summarize_vec2_sequence_diff(&result.controls, &e_baseline_controls)[0];
8115
8116 let ratio = if opp_c0 > 0.0 {
8117 same_c0 / opp_c0
8118 } else {
8119 f64::INFINITY
8120 };
8121
8122 println!(
8123 "{:10.6} {:10.6} {:10.4e} {:10.4e} {:10.3}",
8124 injected_v, result.controls[0][1], same_c0, opp_c0, ratio
8125 );
8126
8127 if threshold_v.is_none() && prev_ratio > 4.0 && ratio <= 4.0 {
8129 threshold_v = Some((injected_v, ratio));
8130 }
8131 prev_ratio = ratio;
8132 }
8133
8134 if let Some((v, r)) = threshold_v {
8135 println!(
8136 "\nThreshold: ratio drops below 4.0 at v={:.6} (ratio={:.3})",
8137 v, r
8138 );
8139 } else {
8140 println!("\nNo threshold crossing found in this range");
8141 }
8142 }
8143}