Skip to main content

rust_robotics_control/
lqr_speed_steer_control.rs

1//! LQR Speed and Steering Control path tracking algorithm
2//!
3//! Extends LQR steer control with speed control, tracking both lateral and
4//! longitudinal errors. The state vector is 5-dimensional:
5//!   x = \[e, dot_e, th_e, dot_th_e, delta_v\]
6//! and the input vector is 2-dimensional:
7//!   u = \[delta, accel\]
8//!
9//! Reference: PythonRobotics lqr_speed_steer_control
10//!
11//! author: Atsushi Sakai (@Atsushi_twi)
12//!         Ryohei Sasaki (@rsasaki0109)
13
14use crate::spline_course::calc_spline_course;
15use nalgebra::{Matrix2, Matrix5, Matrix5x2, SMatrix, SVector, Vector2, Vector5};
16use rust_robotics_core::normalize_angle;
17use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
18use std::f64::consts::PI;
19
20/// Vehicle state for LQR speed+steer controller
21#[derive(Debug, Clone, Copy)]
22pub struct LQRSpeedSteerVehicleState {
23    pub x: f64,
24    pub y: f64,
25    pub yaw: f64,
26    pub v: f64,
27    pub wheelbase: f64,
28    pub max_steer: f64,
29}
30
31impl LQRSpeedSteerVehicleState {
32    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64, max_steer: f64) -> Self {
33        Self {
34            x,
35            y,
36            yaw,
37            v,
38            wheelbase,
39            max_steer,
40        }
41    }
42
43    /// Update vehicle state with bicycle model kinematics
44    pub fn update(&mut self, a: f64, mut delta: f64, dt: f64) {
45        delta = delta.clamp(-self.max_steer, self.max_steer);
46        self.x += self.v * self.yaw.cos() * dt;
47        self.y += self.v * self.yaw.sin() * dt;
48        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
49        self.v += a * dt;
50    }
51
52    pub fn to_state2d(&self) -> State2D {
53        State2D::new(self.x, self.y, self.yaw, self.v)
54    }
55}
56
57impl From<State2D> for LQRSpeedSteerVehicleState {
58    fn from(s: State2D) -> Self {
59        Self::new(s.x, s.y, s.yaw, s.v, 0.5, 45.0_f64.to_radians())
60    }
61}
62
63/// Configuration for LQR Speed and Steer Controller
64#[derive(Debug, Clone)]
65pub struct LQRSpeedSteerConfig {
66    /// Vehicle wheelbase \[m\]
67    pub wheelbase: f64,
68    /// Maximum steering angle \[rad\]
69    pub max_steer: f64,
70    /// State cost matrix Q diagonal (5 elements: e, dot_e, th_e, dot_th_e, delta_v)
71    pub q_diag: [f64; 5],
72    /// Control cost matrix R diagonal (2 elements: delta, accel)
73    pub r_diag: [f64; 2],
74    /// Time step \[s\]
75    pub dt: f64,
76    /// Goal distance threshold \[m\]
77    pub goal_threshold: f64,
78}
79
80impl Default for LQRSpeedSteerConfig {
81    fn default() -> Self {
82        Self {
83            wheelbase: 0.5,
84            max_steer: 45.0_f64.to_radians(),
85            q_diag: [1.0, 1.0, 1.0, 1.0, 1.0],
86            r_diag: [1.0, 1.0],
87            dt: 0.1,
88            goal_threshold: 0.3,
89        }
90    }
91}
92
93/// LQR Speed and Steer path tracking controller
94///
95/// Unlike the steer-only LQR controller, this controller computes both
96/// steering angle and acceleration via a single 5-state LQR formulation,
97/// removing the need for a separate PID speed controller.
98pub struct LQRSpeedSteerController {
99    config: LQRSpeedSteerConfig,
100    path: Path2D,
101    path_yaw: Vec<f64>,
102    path_curvature: Vec<f64>,
103    speed_profile: Vec<f64>,
104    prev_error: f64,
105    prev_theta_error: f64,
106}
107
108impl LQRSpeedSteerController {
109    /// Create a new LQR Speed+Steer controller
110    pub fn new(config: LQRSpeedSteerConfig) -> Self {
111        Self {
112            config,
113            path: Path2D::new(),
114            path_yaw: Vec::new(),
115            path_curvature: Vec::new(),
116            speed_profile: Vec::new(),
117            prev_error: 0.0,
118            prev_theta_error: 0.0,
119        }
120    }
121
122    /// Create with default configuration
123    pub fn with_defaults() -> Self {
124        Self::new(LQRSpeedSteerConfig::default())
125    }
126
127    /// Set the reference path with a default target speed (~10 km/h)
128    pub fn set_path(&mut self, path: Path2D) {
129        let (yaw, curvature) = self.compute_path_derivatives(&path);
130        self.path_yaw = yaw;
131        self.path_curvature = curvature;
132        self.speed_profile = self.calc_speed_profile(&path, 10.0 / 3.6);
133        self.path = path;
134        self.prev_error = 0.0;
135        self.prev_theta_error = 0.0;
136    }
137
138    /// Set the reference path with a specific target speed
139    pub fn set_path_with_speed(&mut self, path: Path2D, target_speed: f64) {
140        let (yaw, curvature) = self.compute_path_derivatives(&path);
141        self.path_yaw = yaw;
142        self.path_curvature = curvature;
143        self.speed_profile = self.calc_speed_profile(&path, target_speed);
144        self.path = path;
145        self.prev_error = 0.0;
146        self.prev_theta_error = 0.0;
147    }
148
149    /// Get the current reference path
150    pub fn get_path(&self) -> &Path2D {
151        &self.path
152    }
153
154    /// Compute yaw and curvature from path points
155    fn compute_path_derivatives(&self, path: &Path2D) -> (Vec<f64>, Vec<f64>) {
156        let n = path.len();
157        let yaw = path.yaw_profile();
158        let mut curvature = Vec::with_capacity(n);
159
160        for i in 0..n {
161            if i > 0 && i < n - 1 {
162                let dyaw = normalize_angle(yaw[i] - yaw[i - 1]);
163                let dx = path.points[i].x - path.points[i - 1].x;
164                let dy = path.points[i].y - path.points[i - 1].y;
165                let ds = (dx * dx + dy * dy).sqrt();
166                curvature.push(if ds > 0.001 { dyaw / ds } else { 0.0 });
167            } else {
168                curvature.push(0.0);
169            }
170        }
171
172        (yaw, curvature)
173    }
174
175    /// Calculate speed profile based on path yaw changes
176    fn calc_speed_profile(&self, path: &Path2D, target_speed: f64) -> Vec<f64> {
177        let n = path.len();
178        let mut profile = vec![target_speed; n];
179        let mut direction = 1.0;
180
181        #[allow(clippy::needless_range_loop)]
182        for i in 0..n.saturating_sub(1) {
183            if i < self.path_yaw.len() - 1 {
184                let dyaw = (self.path_yaw[i + 1] - self.path_yaw[i]).abs();
185                let switch = (PI / 4.0..PI / 2.0).contains(&dyaw);
186
187                if switch {
188                    direction *= -1.0;
189                }
190
191                if direction != 1.0 {
192                    profile[i] = -target_speed;
193                } else {
194                    profile[i] = target_speed;
195                }
196
197                if switch {
198                    profile[i] = 0.0;
199                }
200            }
201        }
202
203        // Speed down near the end
204        let slowdown_len = 40.min(n);
205        for i in 0..slowdown_len {
206            let idx = n - 1 - i;
207            profile[idx] = target_speed / (50 - i) as f64;
208            if profile[idx] <= 1.0 / 3.6 {
209                profile[idx] = 1.0 / 3.6;
210            }
211        }
212
213        profile
214    }
215
216    /// Find nearest path index and signed cross-track error
217    fn calc_nearest_index(&self, state: &LQRSpeedSteerVehicleState) -> (usize, f64) {
218        let query = Point2D::new(state.x, state.y);
219        let min_idx = self.path.nearest_point_index(query).unwrap_or(0);
220        let target = &self.path.points[min_idx];
221        let min_dist = ((state.x - target.x).powi(2) + (state.y - target.y).powi(2)).sqrt();
222
223        let diff_x = target.x - state.x;
224        let diff_y = target.y - state.y;
225        let arcang = self.path_yaw[min_idx] - diff_y.atan2(diff_x);
226        let angle = normalize_angle(arcang);
227
228        let error = if angle < 0.0 { -min_dist } else { min_dist };
229        (min_idx, error)
230    }
231
232    /// Solve Discrete Algebraic Riccati Equation for 5x5 system with 2 inputs
233    fn solve_dare(
234        a: &Matrix5<f64>,
235        b: &Matrix5x2<f64>,
236        q: &Matrix5<f64>,
237        r: &Matrix2<f64>,
238    ) -> Matrix5<f64> {
239        let mut x = *q;
240        let max_iter = 150;
241        let eps = 0.01;
242
243        for _ in 0..max_iter {
244            // x_next = A^T X A - A^T X B (R + B^T X B)^{-1} B^T X A + Q
245            let at = a.transpose();
246            let bt = b.transpose();
247            let at_x = at * x;
248            let at_x_a = at_x * a;
249            let at_x_b = at_x * b;
250            let bt_x_b: Matrix2<f64> = bt * x * b;
251            let inv = (r + bt_x_b).try_inverse().unwrap_or(Matrix2::identity());
252            let bt_x_a: SMatrix<f64, 2, 5> = bt * x * a;
253            let x_next = at_x_a - at_x_b * inv * bt_x_a + q;
254
255            let diff: f64 = (x_next - x).abs().max();
256            if diff < eps {
257                return x_next;
258            }
259            x = x_next;
260        }
261        x
262    }
263
264    /// Compute discrete LQR gain matrix K (2x5)
265    fn dlqr(
266        a: &Matrix5<f64>,
267        b: &Matrix5x2<f64>,
268        q: &Matrix5<f64>,
269        r: &Matrix2<f64>,
270    ) -> SMatrix<f64, 2, 5> {
271        let x = Self::solve_dare(a, b, q, r);
272        let bt = b.transpose();
273        let bt_x_b: Matrix2<f64> = bt * x * b;
274        let inv = (bt_x_b + r).try_inverse().unwrap_or(Matrix2::identity());
275        inv * (bt * x * a)
276    }
277
278    /// Compute LQR speed and steering control
279    ///
280    /// Returns (steering_delta, target_index, lateral_error, heading_error, acceleration)
281    pub fn compute_control_full(
282        &mut self,
283        state: &LQRSpeedSteerVehicleState,
284    ) -> (f64, usize, f64, f64, f64) {
285        if self.path.is_empty() {
286            return (0.0, 0, 0.0, 0.0, 0.0);
287        }
288
289        let (ind, e) = self.calc_nearest_index(state);
290        let tv = if ind < self.speed_profile.len() {
291            self.speed_profile[ind]
292        } else {
293            0.0
294        };
295        let k = if ind < self.path_curvature.len() {
296            self.path_curvature[ind]
297        } else {
298            0.0
299        };
300
301        let v = state.v;
302        let th_e = normalize_angle(state.yaw - self.path_yaw[ind]);
303        let dt = self.config.dt;
304        let l = self.config.wheelbase;
305
306        // Build 5x5 state-space A matrix
307        let mut a = Matrix5::zeros();
308        a[(0, 0)] = 1.0;
309        a[(0, 1)] = dt;
310        a[(1, 2)] = v;
311        a[(2, 2)] = 1.0;
312        a[(2, 3)] = dt;
313        a[(4, 4)] = 1.0;
314
315        // Build 5x2 input B matrix
316        let mut b = Matrix5x2::zeros();
317        b[(3, 0)] = v / l;
318        b[(4, 1)] = dt;
319
320        // Build Q and R matrices
321        let q = Matrix5::from_diagonal(&Vector5::new(
322            self.config.q_diag[0],
323            self.config.q_diag[1],
324            self.config.q_diag[2],
325            self.config.q_diag[3],
326            self.config.q_diag[4],
327        ));
328        let r = Matrix2::from_diagonal(&Vector2::new(self.config.r_diag[0], self.config.r_diag[1]));
329
330        let gain = Self::dlqr(&a, &b, &q, &r);
331
332        // State error vector: [e, dot_e, th_e, dot_th_e, delta_v]
333        let x_err = Vector5::new(
334            e,
335            if dt > 0.0 {
336                (e - self.prev_error) / dt
337            } else {
338                0.0
339            },
340            th_e,
341            if dt > 0.0 {
342                (th_e - self.prev_theta_error) / dt
343            } else {
344                0.0
345            },
346            v - tv,
347        );
348
349        // u* = -K x
350        let ustar: SVector<f64, 2> = -gain * x_err;
351
352        // Steering: feedforward + feedback
353        let ff = (l * k).atan2(1.0);
354        let fb = normalize_angle(ustar[0]);
355        let delta = ff + fb;
356
357        // Acceleration from LQR
358        let accel = ustar[1];
359
360        self.prev_error = e;
361        self.prev_theta_error = th_e;
362
363        let delta_clamped = delta.clamp(-state.max_steer, state.max_steer);
364        (delta_clamped, ind, e, th_e, accel)
365    }
366
367    /// Compute steering angle only (convenience method)
368    pub fn compute_steering(&mut self, state: &LQRSpeedSteerVehicleState) -> f64 {
369        self.compute_control_full(state).0
370    }
371
372    /// Compute acceleration only (convenience method)
373    pub fn compute_acceleration(&mut self, state: &LQRSpeedSteerVehicleState) -> f64 {
374        self.compute_control_full(state).4
375    }
376
377    /// Get target speed at a given index
378    pub fn get_target_speed(&self, index: usize) -> f64 {
379        if index < self.speed_profile.len() {
380            self.speed_profile[index]
381        } else {
382            0.0
383        }
384    }
385
386    /// Check if goal is reached
387    pub fn is_goal_reached(&self, state: &LQRSpeedSteerVehicleState) -> bool {
388        if let Some(goal) = self.path.points.last() {
389            let dx = state.x - goal.x;
390            let dy = state.y - goal.y;
391            (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
392        } else {
393            true
394        }
395    }
396
397    /// Run a full simulation using cubic spline interpolation of waypoints
398    pub fn planning(
399        &mut self,
400        waypoints: Vec<(f64, f64)>,
401        target_speed: f64,
402        ds: f64,
403    ) -> Option<Vec<(f64, f64)>> {
404        if waypoints.len() < 2 {
405            return None;
406        }
407
408        let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
409        let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
410        let (cx, cy, cyaw, ck, _) = calc_spline_course(&ax, &ay, ds);
411
412        let path = Path2D::from_points(
413            cx.iter()
414                .zip(cy.iter())
415                .map(|(&x, &y)| Point2D::new(x, y))
416                .collect(),
417        );
418        self.path_yaw = cyaw;
419        self.path_curvature = ck;
420        self.speed_profile = self.calc_speed_profile(&path, target_speed);
421        self.path = path;
422        self.prev_error = 0.0;
423        self.prev_theta_error = 0.0;
424
425        let mut state = LQRSpeedSteerVehicleState::new(
426            0.0,
427            0.0,
428            0.0,
429            0.0,
430            self.config.wheelbase,
431            self.config.max_steer,
432        );
433
434        let mut trajectory = vec![(state.x, state.y)];
435        let dt = self.config.dt;
436        let t_max = 500.0;
437        let stop_speed = 0.05;
438        let mut time = 0.0;
439
440        while time < t_max {
441            let (delta, target_ind, _, _, accel) = self.compute_control_full(&state);
442
443            state.update(accel, delta, dt);
444            time += dt;
445
446            if state.v.abs() <= stop_speed && target_ind + 1 < self.path.len() {
447                // nudge forward when nearly stopped
448            }
449
450            trajectory.push((state.x, state.y));
451
452            if self.is_goal_reached(&state) {
453                break;
454            }
455        }
456
457        Some(trajectory)
458    }
459}
460
461impl PathTracker for LQRSpeedSteerController {
462    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
463        // Adopt a new reference path (comparing contents, not just length)
464        if self.path != *path {
465            self.set_path(path.clone());
466        }
467
468        let vehicle_state = LQRSpeedSteerVehicleState::new(
469            current_state.x,
470            current_state.y,
471            current_state.yaw,
472            current_state.v,
473            self.config.wheelbase,
474            self.config.max_steer,
475        );
476
477        let (delta, _, _, _, accel) = self.compute_control_full(&vehicle_state);
478
479        let v = current_state.v + accel * self.config.dt;
480        let omega = v * delta.tan() / self.config.wheelbase;
481
482        ControlInput::new(v, omega)
483    }
484
485    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
486        let dx = current_state.x - goal.x;
487        let dy = current_state.y - goal.y;
488        (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
489    }
490}
491
492#[cfg(test)]
493mod tests {
494    use super::*;
495
496    #[test]
497    fn test_creation() {
498        let config = LQRSpeedSteerConfig::default();
499        let controller = LQRSpeedSteerController::new(config);
500        assert!(controller.path.is_empty());
501    }
502
503    #[test]
504    fn test_default_config() {
505        let config = LQRSpeedSteerConfig::default();
506        assert_eq!(config.q_diag.len(), 5);
507        assert_eq!(config.r_diag.len(), 2);
508        assert!((config.wheelbase - 0.5).abs() < 1e-9);
509        assert!((config.dt - 0.1).abs() < 1e-9);
510    }
511
512    #[test]
513    fn test_set_path() {
514        let mut controller = LQRSpeedSteerController::with_defaults();
515        let path = Path2D::from_points(vec![
516            Point2D::new(0.0, 0.0),
517            Point2D::new(1.0, 0.0),
518            Point2D::new(2.0, 0.0),
519            Point2D::new(3.0, 0.0),
520        ]);
521        controller.set_path(path);
522        assert_eq!(controller.get_path().len(), 4);
523        assert_eq!(controller.path_yaw.len(), 4);
524        assert_eq!(controller.path_curvature.len(), 4);
525        assert_eq!(controller.speed_profile.len(), 4);
526    }
527
528    #[test]
529    fn test_set_path_with_speed() {
530        let mut controller = LQRSpeedSteerController::with_defaults();
531        let path = Path2D::from_points(vec![
532            Point2D::new(0.0, 0.0),
533            Point2D::new(1.0, 0.0),
534            Point2D::new(2.0, 0.0),
535        ]);
536        controller.set_path_with_speed(path, 5.0);
537        assert_eq!(controller.get_path().len(), 3);
538    }
539
540    #[test]
541    fn test_normalize_angle() {
542        let a = normalize_angle(3.0 * PI);
543        assert!((a - PI).abs() < 0.01);
544        let b = normalize_angle(-3.0 * PI);
545        assert!((b + PI).abs() < 0.01);
546        let c = normalize_angle(0.5);
547        assert!((c - 0.5).abs() < 1e-9);
548    }
549
550    #[test]
551    fn test_solve_dare_identity() {
552        let a = Matrix5::identity();
553        let mut b = Matrix5x2::zeros();
554        b[(3, 0)] = 1.0;
555        b[(4, 1)] = 0.1;
556        let q = Matrix5::identity();
557        let r = Matrix2::identity();
558
559        let x = LQRSpeedSteerController::solve_dare(&a, &b, &q, &r);
560        // Result should be positive semi-definite (diagonal >= 0)
561        for i in 0..5 {
562            assert!(x[(i, i)] >= 0.0);
563        }
564    }
565
566    #[test]
567    fn test_dlqr_gain() {
568        let a = Matrix5::identity();
569        let mut b = Matrix5x2::zeros();
570        b[(3, 0)] = 1.0;
571        b[(4, 1)] = 0.1;
572        let q = Matrix5::identity();
573        let r = Matrix2::identity();
574
575        let k = LQRSpeedSteerController::dlqr(&a, &b, &q, &r);
576        // K should be 2x5
577        assert_eq!(k.nrows(), 2);
578        assert_eq!(k.ncols(), 5);
579    }
580
581    #[test]
582    fn test_compute_control_empty_path() {
583        let mut controller = LQRSpeedSteerController::with_defaults();
584        let state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
585        let (delta, ind, e, th_e, accel) = controller.compute_control_full(&state);
586        assert_eq!(delta, 0.0);
587        assert_eq!(ind, 0);
588        assert_eq!(e, 0.0);
589        assert_eq!(th_e, 0.0);
590        assert_eq!(accel, 0.0);
591    }
592
593    #[test]
594    fn test_compute_control_straight_path() {
595        let mut controller = LQRSpeedSteerController::with_defaults();
596        let path =
597            Path2D::from_points((0..50).map(|i| Point2D::new(i as f64 * 0.5, 0.0)).collect());
598        controller.set_path_with_speed(path, 2.0);
599
600        let state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
601        let (delta, _ind, _e, _th_e, _accel) = controller.compute_control_full(&state);
602        // On a straight path aligned with the vehicle, steering should be near zero
603        assert!(delta.abs() < 0.5);
604    }
605
606    #[test]
607    fn test_vehicle_state_update() {
608        let mut state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
609        state.update(0.0, 0.0, 0.1);
610        assert!((state.x - 0.1).abs() < 1e-9);
611        assert!(state.y.abs() < 1e-9);
612    }
613
614    #[test]
615    fn test_vehicle_state_update_with_steer() {
616        let mut state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
617        state.update(0.0, 0.1, 0.1);
618        // Should have moved forward and slightly turned
619        assert!(state.x > 0.0);
620        assert!(state.yaw.abs() > 0.0);
621    }
622
623    #[test]
624    fn test_vehicle_state_steer_clamping() {
625        let mut state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.1);
626        state.update(0.0, 1.0, 0.1); // delta=1.0 exceeds max_steer=0.1
627                                     // yaw change should be limited by max_steer
628        let expected_yaw = 1.0 / 0.5 * (0.1_f64).tan() * 0.1;
629        assert!((state.yaw - expected_yaw).abs() < 1e-9);
630    }
631
632    #[test]
633    fn test_from_state2d() {
634        let s = State2D::new(1.0, 2.0, 0.5, 3.0);
635        let vs = LQRSpeedSteerVehicleState::from(s);
636        assert!((vs.x - 1.0).abs() < 1e-9);
637        assert!((vs.y - 2.0).abs() < 1e-9);
638        assert!((vs.yaw - 0.5).abs() < 1e-9);
639        assert!((vs.v - 3.0).abs() < 1e-9);
640    }
641
642    #[test]
643    fn test_to_state2d() {
644        let vs = LQRSpeedSteerVehicleState::new(1.0, 2.0, 0.5, 3.0, 0.5, 0.78);
645        let s = vs.to_state2d();
646        assert!((s.x - 1.0).abs() < 1e-9);
647        assert!((s.y - 2.0).abs() < 1e-9);
648    }
649
650    #[test]
651    fn test_is_goal_reached() {
652        let mut controller = LQRSpeedSteerController::with_defaults();
653        let path = Path2D::from_points(vec![Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)]);
654        controller.set_path(path);
655
656        let far = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
657        assert!(!controller.is_goal_reached(&far));
658
659        let near = LQRSpeedSteerVehicleState::new(5.0, 0.1, 0.0, 0.0, 0.5, 0.78);
660        assert!(controller.is_goal_reached(&near));
661    }
662
663    #[test]
664    fn test_planning_straight_line() {
665        let mut controller = LQRSpeedSteerController::with_defaults();
666        let waypoints = vec![(0.0, 0.0), (5.0, 0.0), (10.0, 0.0)];
667        let result = controller.planning(waypoints, 2.0, 0.5);
668        assert!(result.is_some());
669        let traj = result.unwrap();
670        assert!(!traj.is_empty());
671        // Final position should be near the goal
672        let (fx, fy) = traj.last().unwrap();
673        let dist = ((fx - 10.0).powi(2) + fy.powi(2)).sqrt();
674        assert!(dist < 2.0, "Final distance to goal: {}", dist);
675    }
676
677    #[test]
678    fn test_planning_with_curve() {
679        let mut controller = LQRSpeedSteerController::with_defaults();
680        let waypoints = vec![
681            (0.0, 0.0),
682            (6.0, -3.0),
683            (12.5, -5.0),
684            (10.0, 6.5),
685            (17.5, 3.0),
686            (20.0, 0.0),
687            (25.0, 0.0),
688        ];
689        let result = controller.planning(waypoints, 10.0 / 3.6, 0.1);
690        assert!(result.is_some());
691        let traj = result.unwrap();
692        assert!(traj.len() > 10);
693    }
694
695    #[test]
696    fn test_planning_too_few_waypoints() {
697        let mut controller = LQRSpeedSteerController::with_defaults();
698        let result = controller.planning(vec![(0.0, 0.0)], 2.0, 0.5);
699        assert!(result.is_none());
700    }
701
702    #[test]
703    fn test_path_tracker_trait() {
704        let mut controller = LQRSpeedSteerController::with_defaults();
705        let path =
706            Path2D::from_points((0..50).map(|i| Point2D::new(i as f64 * 0.5, 0.0)).collect());
707        let state = State2D::new(0.0, 0.0, 0.0, 1.0);
708        let input = controller.compute_control(&state, &path);
709        // Should produce some valid control input
710        assert!(input.v.is_finite());
711        assert!(input.omega.is_finite());
712    }
713
714    #[test]
715    fn test_speed_profile_slowdown() {
716        let mut controller = LQRSpeedSteerController::with_defaults();
717        let path = Path2D::from_points(
718            (0..100)
719                .map(|i| Point2D::new(i as f64 * 0.1, 0.0))
720                .collect(),
721        );
722        controller.set_path_with_speed(path, 3.0);
723        // Speed near the end should be less than target speed
724        let n = controller.speed_profile.len();
725        assert!(controller.speed_profile[n - 1] < 3.0);
726        assert!(controller.speed_profile[n - 1] > 0.0);
727    }
728
729    #[test]
730    fn test_get_target_speed() {
731        let mut controller = LQRSpeedSteerController::with_defaults();
732        let path = Path2D::from_points(vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)]);
733        controller.set_path_with_speed(path, 2.0);
734        // Valid index
735        let s = controller.get_target_speed(0);
736        assert!(s.is_finite());
737        // Out of bounds should return 0
738        assert_eq!(controller.get_target_speed(1000), 0.0);
739    }
740}