Skip to main content

rust_robotics_control/
ilqr.rs

1//! Iterative Linear Quadratic Regulator (iLQR) for a unicycle model.
2//!
3//! The planner optimizes a finite-horizon state and control trajectory to
4//! drive a robot from a start pose to a goal pose.
5//!
6//! Reference:
7//! - Weiwei Li, Emanuel Todorov, "Iterative Linear Quadratic Regulator
8//!   Design for Nonlinear Biological Movement Systems":
9//!   <https://homes.cs.washington.edu/~todorov/papers/LiICINCO04.pdf>
10
11use nalgebra::{Matrix2, Matrix2x3, Matrix3, Matrix3x2, Vector2, Vector3};
12use rust_robotics_core::normalize_angle;
13use std::f64::consts::PI;
14
15const MAX_LINEAR_SPEED: f64 = 5.0;
16const MAX_ANGULAR_SPEED: f64 = PI;
17const TERMINAL_WEIGHT_SCALE: f64 = 10.0;
18const REGULARIZATION: f64 = 1.0e-6;
19
20type FeedforwardTerms = Vec<Vector2<f64>>;
21type FeedbackGains = Vec<Matrix2x3<f64>>;
22
23/// Configuration for the iLQR planner.
24#[derive(Debug, Clone, Copy)]
25pub struct ILQRConfig {
26    /// Number of time steps in the optimization horizon.
27    pub horizon: usize,
28    /// Time step \[s\].
29    pub dt: f64,
30    /// Maximum iLQR iterations.
31    pub max_iterations: usize,
32    /// Convergence threshold on cost change.
33    pub tolerance: f64,
34    /// Position cost weight.
35    pub q_pos: f64,
36    /// Heading cost weight.
37    pub q_yaw: f64,
38    /// Linear velocity input cost weight.
39    pub r_v: f64,
40    /// Angular velocity input cost weight.
41    pub r_omega: f64,
42}
43
44impl Default for ILQRConfig {
45    fn default() -> Self {
46        Self {
47            horizon: 50,
48            dt: 0.1,
49            max_iterations: 50,
50            tolerance: 1.0e-6,
51            q_pos: 1.0,
52            q_yaw: 0.1,
53            r_v: 0.01,
54            r_omega: 0.01,
55        }
56    }
57}
58
59/// Optimized iLQR trajectory.
60#[derive(Debug, Clone)]
61pub struct ILQRResult {
62    pub states: Vec<Vector3<f64>>,
63    pub controls: Vec<Vector2<f64>>,
64    pub cost: f64,
65    pub iterations: usize,
66    pub converged: bool,
67}
68
69/// iLQR planner for a unicycle model.
70pub struct ILQRPlanner {
71    config: ILQRConfig,
72}
73
74impl ILQRPlanner {
75    /// Creates a new iLQR planner.
76    pub fn new(config: ILQRConfig) -> Self {
77        Self { config }
78    }
79
80    /// Plans a trajectory from `start` to `goal`.
81    pub fn plan(&self, start: Vector3<f64>, goal: Vector3<f64>) -> ILQRResult {
82        let mut controls = self.initial_controls(start, goal);
83        let mut states = self.rollout(start, &controls);
84        let mut cost = self.total_cost(&states, &controls, goal);
85        let mut iterations = 0;
86        let mut converged = false;
87
88        for iter in 0..self.config.max_iterations {
89            iterations = iter + 1;
90            let Some((feedforward, feedback)) = self.backward_pass(&states, &controls, goal) else {
91                break;
92            };
93
94            let mut accepted = None;
95            for alpha in [1.0, 0.5, 0.25, 0.1, 0.05, 0.01] {
96                let (candidate_states, candidate_controls) =
97                    self.forward_pass(start, &states, &controls, &feedforward, &feedback, alpha);
98                let candidate_cost = self.total_cost(&candidate_states, &candidate_controls, goal);
99
100                if candidate_cost < cost {
101                    accepted = Some((candidate_states, candidate_controls, candidate_cost));
102                    break;
103                }
104            }
105
106            let Some((candidate_states, candidate_controls, candidate_cost)) = accepted else {
107                break;
108            };
109
110            let cost_change = (cost - candidate_cost).abs();
111            states = candidate_states;
112            controls = candidate_controls;
113            cost = candidate_cost;
114
115            if cost_change < self.config.tolerance {
116                converged = true;
117                break;
118            }
119        }
120
121        if !converged {
122            let final_state = states.last().copied().unwrap_or(start);
123            let pos_error =
124                ((goal[0] - final_state[0]).powi(2) + (goal[1] - final_state[1]).powi(2)).sqrt();
125            let yaw_error = normalize_angle(goal[2] - final_state[2]).abs();
126            converged = pos_error < 0.25 && yaw_error < 0.1;
127        }
128
129        ILQRResult {
130            states,
131            controls,
132            cost,
133            iterations,
134            converged,
135        }
136    }
137
138    fn initial_controls(&self, start: Vector3<f64>, goal: Vector3<f64>) -> Vec<Vector2<f64>> {
139        let distance = ((goal[0] - start[0]).powi(2) + (goal[1] - start[1]).powi(2)).sqrt();
140        let total_time = (self.config.horizon as f64 * self.config.dt).max(self.config.dt);
141        let v = (distance / total_time).clamp(-MAX_LINEAR_SPEED, MAX_LINEAR_SPEED);
142        let omega = (normalize_angle(goal[2] - start[2]) / total_time)
143            .clamp(-MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED);
144        vec![Vector2::new(v, omega); self.config.horizon]
145    }
146
147    fn rollout(&self, start: Vector3<f64>, controls: &[Vector2<f64>]) -> Vec<Vector3<f64>> {
148        let mut states = Vec::with_capacity(controls.len() + 1);
149        let mut state = start;
150        states.push(state);
151
152        for control in controls {
153            state = dynamics(&state, control, self.config.dt);
154            states.push(state);
155        }
156
157        states
158    }
159
160    fn total_cost(
161        &self,
162        states: &[Vector3<f64>],
163        controls: &[Vector2<f64>],
164        goal: Vector3<f64>,
165    ) -> f64 {
166        let stage_cost: f64 = states
167            .iter()
168            .take(controls.len())
169            .zip(controls.iter())
170            .map(|(state, control)| self.stage_cost(state, control, goal))
171            .sum();
172        stage_cost + self.terminal_cost(states.last().expect("trajectory is non-empty"), goal)
173    }
174
175    fn stage_cost(&self, state: &Vector3<f64>, control: &Vector2<f64>, goal: Vector3<f64>) -> f64 {
176        let error = state_error(state, goal);
177        self.config.q_pos * (error[0].powi(2) + error[1].powi(2))
178            + self.config.q_yaw * error[2].powi(2)
179            + self.config.r_v * control[0].powi(2)
180            + self.config.r_omega * control[1].powi(2)
181    }
182
183    fn terminal_cost(&self, state: &Vector3<f64>, goal: Vector3<f64>) -> f64 {
184        let error = state_error(state, goal);
185        TERMINAL_WEIGHT_SCALE
186            * (self.config.q_pos * (error[0].powi(2) + error[1].powi(2))
187                + self.config.q_yaw * error[2].powi(2))
188    }
189
190    fn backward_pass(
191        &self,
192        states: &[Vector3<f64>],
193        controls: &[Vector2<f64>],
194        goal: Vector3<f64>,
195    ) -> Option<(FeedforwardTerms, FeedbackGains)> {
196        let horizon = controls.len();
197        let mut feedforward = vec![Vector2::zeros(); horizon];
198        let mut feedback = vec![Matrix2x3::<f64>::zeros(); horizon];
199
200        let (mut value_x, mut value_xx) =
201            self.terminal_cost_derivatives(states.last().expect("trajectory is non-empty"), goal);
202
203        for t in (0..horizon).rev() {
204            let (a, b) = linearize_dynamics(&states[t], &controls[t], self.config.dt);
205            let (l_x, l_u, l_xx, l_uu) =
206                self.stage_cost_derivatives(&states[t], &controls[t], goal);
207
208            let q_x = l_x + a.transpose() * value_x;
209            let q_u = l_u + b.transpose() * value_x;
210            let q_xx = l_xx + a.transpose() * value_xx * a;
211            let mut q_uu = l_uu + b.transpose() * value_xx * b;
212            let q_ux = b.transpose() * value_xx * a;
213
214            q_uu = symmetrize_2x2(q_uu) + REGULARIZATION * Matrix2::identity();
215            let q_uu_inv = q_uu.try_inverse()?;
216
217            let k = -q_uu_inv * q_u;
218            let k_feedback = -q_uu_inv * q_ux;
219            feedforward[t] = k;
220            feedback[t] = k_feedback;
221
222            value_x = q_x
223                + k_feedback.transpose() * q_uu * k
224                + k_feedback.transpose() * q_u
225                + q_ux.transpose() * k;
226            value_xx = q_xx
227                + k_feedback.transpose() * q_uu * k_feedback
228                + k_feedback.transpose() * q_ux
229                + q_ux.transpose() * k_feedback;
230            value_xx = symmetrize_3x3(value_xx);
231        }
232
233        Some((feedforward, feedback))
234    }
235
236    fn forward_pass(
237        &self,
238        start: Vector3<f64>,
239        nominal_states: &[Vector3<f64>],
240        nominal_controls: &[Vector2<f64>],
241        feedforward: &[Vector2<f64>],
242        feedback: &[Matrix2x3<f64>],
243        alpha: f64,
244    ) -> (Vec<Vector3<f64>>, Vec<Vector2<f64>>) {
245        let mut states = Vec::with_capacity(nominal_controls.len() + 1);
246        let mut controls = Vec::with_capacity(nominal_controls.len());
247        states.push(start);
248
249        for t in 0..nominal_controls.len() {
250            let delta_state = state_error(&states[t], nominal_states[t]);
251            let delta_control = alpha * feedforward[t] + feedback[t] * delta_state;
252            let mut control = nominal_controls[t] + delta_control;
253            control[0] = control[0].clamp(-MAX_LINEAR_SPEED, MAX_LINEAR_SPEED);
254            control[1] = control[1].clamp(-MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED);
255            controls.push(control);
256
257            let next_state = dynamics(&states[t], &control, self.config.dt);
258            states.push(next_state);
259        }
260
261        (states, controls)
262    }
263
264    fn stage_cost_derivatives(
265        &self,
266        state: &Vector3<f64>,
267        control: &Vector2<f64>,
268        goal: Vector3<f64>,
269    ) -> (Vector3<f64>, Vector2<f64>, Matrix3<f64>, Matrix2<f64>) {
270        let error = state_error(state, goal);
271        let l_x = Vector3::new(
272            2.0 * self.config.q_pos * error[0],
273            2.0 * self.config.q_pos * error[1],
274            2.0 * self.config.q_yaw * error[2],
275        );
276        let l_u = Vector2::new(
277            2.0 * self.config.r_v * control[0],
278            2.0 * self.config.r_omega * control[1],
279        );
280        let l_xx = Matrix3::from_diagonal(&Vector3::new(
281            2.0 * self.config.q_pos,
282            2.0 * self.config.q_pos,
283            2.0 * self.config.q_yaw,
284        ));
285        let l_uu = Matrix2::from_diagonal(&Vector2::new(
286            2.0 * self.config.r_v,
287            2.0 * self.config.r_omega,
288        ));
289
290        (l_x, l_u, l_xx, l_uu)
291    }
292
293    fn terminal_cost_derivatives(
294        &self,
295        state: &Vector3<f64>,
296        goal: Vector3<f64>,
297    ) -> (Vector3<f64>, Matrix3<f64>) {
298        let error = state_error(state, goal);
299        let v_x = Vector3::new(
300            2.0 * TERMINAL_WEIGHT_SCALE * self.config.q_pos * error[0],
301            2.0 * TERMINAL_WEIGHT_SCALE * self.config.q_pos * error[1],
302            2.0 * TERMINAL_WEIGHT_SCALE * self.config.q_yaw * error[2],
303        );
304        let v_xx = Matrix3::from_diagonal(&Vector3::new(
305            2.0 * TERMINAL_WEIGHT_SCALE * self.config.q_pos,
306            2.0 * TERMINAL_WEIGHT_SCALE * self.config.q_pos,
307            2.0 * TERMINAL_WEIGHT_SCALE * self.config.q_yaw,
308        ));
309
310        (v_x, v_xx)
311    }
312}
313
314fn state_error(state: &Vector3<f64>, goal: Vector3<f64>) -> Vector3<f64> {
315    Vector3::new(
316        state[0] - goal[0],
317        state[1] - goal[1],
318        normalize_angle(state[2] - goal[2]),
319    )
320}
321
322fn symmetrize_2x2(matrix: Matrix2<f64>) -> Matrix2<f64> {
323    0.5 * (matrix + matrix.transpose())
324}
325
326fn symmetrize_3x3(matrix: Matrix3<f64>) -> Matrix3<f64> {
327    0.5 * (matrix + matrix.transpose())
328}
329
330fn dynamics(state: &Vector3<f64>, control: &Vector2<f64>, dt: f64) -> Vector3<f64> {
331    let x = state[0] + control[0] * state[2].cos() * dt;
332    let y = state[1] + control[0] * state[2].sin() * dt;
333    let yaw = normalize_angle(state[2] + control[1] * dt);
334    Vector3::new(x, y, yaw)
335}
336
337fn linearize_dynamics(
338    state: &Vector3<f64>,
339    control: &Vector2<f64>,
340    dt: f64,
341) -> (Matrix3<f64>, Matrix3x2<f64>) {
342    let yaw = state[2];
343    let v = control[0];
344
345    let a = Matrix3::new(
346        1.0,
347        0.0,
348        -dt * v * yaw.sin(),
349        0.0,
350        1.0,
351        dt * v * yaw.cos(),
352        0.0,
353        0.0,
354        1.0,
355    );
356    let b = Matrix3x2::new(dt * yaw.cos(), 0.0, dt * yaw.sin(), 0.0, 0.0, dt);
357
358    (a, b)
359}
360
361#[cfg(test)]
362mod tests {
363    use super::*;
364
365    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
366        assert!(
367            (actual - expected).abs() <= tolerance,
368            "expected {expected:.12}, got {actual:.12}, tolerance {tolerance:.3e}"
369        );
370    }
371
372    #[test]
373    fn test_ilqr_config_defaults() {
374        let config = ILQRConfig::default();
375        assert_eq!(config.horizon, 50);
376        assert_eq!(config.dt, 0.1);
377        assert_eq!(config.max_iterations, 50);
378        assert_eq!(config.tolerance, 1.0e-6);
379        assert_eq!(config.q_pos, 1.0);
380        assert_eq!(config.q_yaw, 0.1);
381        assert_eq!(config.r_v, 0.01);
382        assert_eq!(config.r_omega, 0.01);
383    }
384
385    #[test]
386    fn test_ilqr_straight_line_converges() {
387        let planner = ILQRPlanner::new(ILQRConfig {
388            horizon: 60,
389            ..ILQRConfig::default()
390        });
391        let start = Vector3::new(0.0, 0.0, 0.0);
392        let goal = Vector3::new(5.0, 0.0, 0.0);
393
394        let result = planner.plan(start, goal);
395        let final_state = result.states.last().expect("trajectory").clone_owned();
396
397        assert!(result.converged);
398        assert!((final_state[0] - goal[0]).abs() < 0.25);
399        assert!(final_state[1].abs() < 0.1);
400        assert!(normalize_angle(final_state[2] - goal[2]).abs() < 0.1);
401    }
402
403    #[test]
404    fn test_ilqr_turn_converges() {
405        let planner = ILQRPlanner::new(ILQRConfig {
406            horizon: 70,
407            ..ILQRConfig::default()
408        });
409        let start = Vector3::new(0.0, 0.0, 0.0);
410        let goal = Vector3::new(3.0, 2.0, PI / 2.0);
411
412        let result = planner.plan(start, goal);
413        let final_state = result.states.last().expect("trajectory").clone_owned();
414        let pos_error =
415            ((final_state[0] - goal[0]).powi(2) + (final_state[1] - goal[1]).powi(2)).sqrt();
416
417        assert!(result.converged);
418        assert!(pos_error < 0.3);
419        assert!(normalize_angle(final_state[2] - goal[2]).abs() < 0.15);
420    }
421
422    #[test]
423    fn test_ilqr_cost_decreases_over_iterations() {
424        let planner = ILQRPlanner::new(ILQRConfig::default());
425        let start = Vector3::new(0.0, 0.0, 0.0);
426        let goal = Vector3::new(4.0, 1.0, 0.2);
427
428        let initial_controls = planner.initial_controls(start, goal);
429        let initial_states = planner.rollout(start, &initial_controls);
430        let initial_cost = planner.total_cost(&initial_states, &initial_controls, goal);
431
432        let result = planner.plan(start, goal);
433
434        assert!(result.cost < initial_cost);
435    }
436
437    #[test]
438    fn test_ilqr_turning_case_regression() {
439        let planner = ILQRPlanner::new(ILQRConfig {
440            horizon: 70,
441            ..ILQRConfig::default()
442        });
443        let start = Vector3::new(0.0, 0.0, 0.0);
444        let goal = Vector3::new(3.0, 2.0, PI / 2.0);
445
446        let result = planner.plan(start, goal);
447        let final_state = result.states.last().expect("trajectory").clone_owned();
448
449        assert!(result.converged);
450        assert_eq!(result.iterations, 10);
451        assert_close(result.cost, 43.962457904378, 1.0e-6);
452        assert_close(final_state[0], 2.991948883770, 1.0e-6);
453        assert_close(final_state[1], 2.000000028451, 1.0e-6);
454        assert_close(final_state[2], 1.570796059231, 1.0e-6);
455        assert_close(result.controls[0][0], 5.0, 1.0e-12);
456        assert_close(result.controls[0][1], PI, 1.0e-12);
457        assert_close(result.controls[1][0], 5.0, 1.0e-12);
458        assert_close(result.controls[1][1], PI, 1.0e-12);
459    }
460}