1use 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#[derive(Debug, Clone, Copy)]
25pub struct ILQRConfig {
26 pub horizon: usize,
28 pub dt: f64,
30 pub max_iterations: usize,
32 pub tolerance: f64,
34 pub q_pos: f64,
36 pub q_yaw: f64,
38 pub r_v: f64,
40 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#[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
69pub struct ILQRPlanner {
71 config: ILQRConfig,
72}
73
74impl ILQRPlanner {
75 pub fn new(config: ILQRConfig) -> Self {
77 Self { config }
78 }
79
80 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}