rust_robotics_control/
model_predictive_trajectory_generator.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3// https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning/ModelPredictiveTrajectoryGenerator
4//
5// Model trajectory generator
6//
7// author: Atsushi Sakai(@Atsushi_twi)
8//         Ryohei Sasaki(@rsasaki0109)
9
10fn pi2pi(mut angle: f64) -> f64 {
11    while angle > std::f64::consts::PI {
12        angle -= 2. * std::f64::consts::PI
13    }
14
15    while angle < -std::f64::consts::PI {
16        angle += 2. * std::f64::consts::PI
17    }
18    angle
19}
20
21pub struct State {
22    x: f64,
23    y: f64,
24    yaw: f64,
25    v: f64,
26    l: f64,
27}
28
29impl State {
30    fn new(x: (f64, f64, f64, f64), l: f64) -> State {
31        State {
32            x: x.0,
33            y: x.1,
34            yaw: x.2,
35            v: x.3,
36            l,
37        }
38    }
39
40    fn update(&mut self, v: f64, delta: f64, dt: f64) {
41        self.v = v;
42        self.x += self.v * (self.yaw).cos() * dt;
43        self.y += self.v * (self.yaw).sin() * dt;
44        self.yaw += self.v / self.l * (delta).tan() * dt;
45        self.yaw = pi2pi(self.yaw);
46    }
47}
48
49pub fn generate_trajectory(
50    p: (f64, f64, f64),
51    k0: f64,
52    ds: f64,
53    v: f64,
54    l: f64,
55) -> (Vec<(f64, f64)>, Vec<f64>) {
56    let s = p.0;
57    let km = p.1;
58    let kf = p.2;
59
60    let n = (s / ds) as usize;
61    let time = s / v;
62
63    let tk = (0., time / 2., time);
64    let kk = (k0, km, kf);
65    let coef = quad_interp(tk, kk);
66
67    let init_x = (0., 0., 0., 0.); // [x, y, yaw, v]
68    let mut state: State = State::new(init_x, l);
69    let mut c = vec![(state.x, state.y)];
70    let mut cyaw = vec![(state.yaw)];
71
72    let dt = time / (n as f64);
73    for i in 0..n - 1 {
74        let x = (i as f64) * dt;
75        let ikp = coef.0 * x * x + coef.1 * x + coef.2;
76        state.update(v, ikp, dt);
77        c.push((state.x, state.y));
78        cyaw.push(state.yaw);
79    }
80    (c, cyaw)
81}
82
83pub fn generate_last_state(
84    p: (f64, f64, f64),
85    k0: f64,
86    ds: f64,
87    v: f64,
88    l: f64,
89) -> ((f64, f64), f64) {
90    let s = p.0;
91    let km = p.1;
92    let kf = p.2;
93
94    let n = (s / ds) as usize;
95    let time = s / v;
96
97    let tk = (0., time / 2., time);
98    let kk = (k0, km, kf);
99    let coef = quad_interp(tk, kk);
100
101    let init_x = (0., 0., 0., 0.); // [x, y, yaw, v]
102    let mut state: State = State::new(init_x, l);
103
104    let dt = time / (n as f64);
105    for i in 0..n - 1 {
106        let t = (i as f64) * dt;
107        let ikp = coef.0 * t * t + coef.1 * t + coef.2;
108        state.update(v, ikp, dt);
109    }
110    ((state.x, state.y), state.yaw)
111}
112
113fn quad_interp(x: (f64, f64, f64), y: (f64, f64, f64)) -> (f64, f64, f64) {
114    let mat = nalgebra::Matrix3::new(x.0 * x.0, x.0, 1., x.1 * x.1, x.1, 1., x.2 * x.2, x.2, 1.);
115    let vec = nalgebra::Vector3::new(y.0, y.1, y.2);
116
117    // The Vandermonde-like matrix is invertible when x values are distinct
118    let coef = mat
119        .try_inverse()
120        .expect("interpolation matrix is singular; x values must be distinct")
121        * vec;
122
123    (coef[0], coef[1], coef[2])
124}
125
126fn calc_diff(target: &State, x: (f64, f64, f64)) -> nalgebra::Vector3<f64> {
127    nalgebra::Vector3::new(target.x - x.0, target.y - x.1, pi2pi(target.yaw - x.2))
128}
129
130fn calc_j(
131    target: &State,
132    p: nalgebra::Vector3<f64>,
133    h: nalgebra::Vector3<f64>,
134    k0: f64,
135    ds: f64,
136    v: f64,
137    l: f64,
138) -> nalgebra::Matrix3<f64> {
139    let (mut xp, mut yawp) = generate_last_state((p[0] + h[0], p[1], p[2]), k0, ds, v, l);
140    let mut dp = calc_diff(target, (xp.0, xp.1, yawp));
141    let (mut xn, mut yawn) = generate_last_state((p[0] - h[0], p[1], p[2]), k0, ds, v, l);
142    let mut dn = calc_diff(target, (xn.0, xn.1, yawn));
143    let d1 = (dp - dn) / (2. * h[0]);
144
145    let mut pair = generate_last_state((p[0], p[1] + h[1], p[2]), k0, ds, v, l);
146    xp = pair.0;
147    yawp = pair.1;
148    dp = calc_diff(target, (xp.0, xp.1, yawp));
149    pair = generate_last_state((p[0], p[1] - h[1], p[2]), k0, ds, v, l);
150    xn = pair.0;
151    yawn = pair.1;
152    dn = calc_diff(target, (xn.0, xn.1, yawn));
153    let d2 = (dp - dn) / (2. * h[1]);
154
155    pair = generate_last_state((p[0], p[1], p[2] + h[2]), k0, ds, v, l);
156    xp = pair.0;
157    yawp = pair.1;
158    dp = calc_diff(target, (xp.0, xp.1, yawp));
159    pair = generate_last_state((p[0], p[1], p[2] - h[2]), k0, ds, v, l);
160    xn = pair.0;
161    yawn = pair.1;
162    dn = calc_diff(target, (xn.0, xn.1, yawn));
163    let d3 = (dp - dn) / (2. * h[2]);
164
165    nalgebra::Matrix3::new(
166        d1[0], d2[0], d3[0], d1[1], d2[1], d3[1], d1[2], d2[2], d3[2],
167    )
168}
169
170fn selection_learning_param(
171    p: nalgebra::Vector3<f64>,
172    k0: f64,
173    ds: f64,
174    v: f64,
175    l: f64,
176    target: &State,
177) -> f64 {
178    let mut mincost = f64::MAX;
179    let mut mina = 1.0;
180    let maxa = 2.0;
181    let da = 0.5;
182    let na = ((maxa - mina) / da) as usize;
183    for i in 0..na - 1 {
184        let a = mina + (na as f64) * (i as f64);
185        let tp = p + a * p;
186        let pair = generate_last_state((tp[0], tp[1], tp[2]), k0, ds, v, l);
187        let xc = pair.0;
188        let yawc = pair.1;
189        let dc = calc_diff(target, (xc.0, xc.1, yawc));
190        let cost = dc.norm();
191        if cost <= mincost && a != 0. {
192            mina = a;
193            mincost = cost;
194        }
195    }
196    mina
197}
198
199pub fn optimize_trajectory(
200    target: State,
201    mut p: nalgebra::Vector3<f64>,
202    h: nalgebra::Vector3<f64>,
203    k0: f64,
204    ds: f64,
205    v: f64,
206    l: f64,
207    max_iter: usize,
208    cost_th: f64,
209) -> (Vec<(f64, f64)>, Vec<f64>, nalgebra::Vector3<f64>) {
210    let mut xc: Vec<(f64, f64)> = vec![(0., 0.)];
211    let mut yawc: Vec<f64> = vec![0.];
212    for _i in 0..max_iter - 1 {
213        let pair = generate_trajectory((p[0], p[1], p[2]), k0, ds, v, l);
214        xc = pair.0;
215        yawc = pair.1;
216        let dc = calc_diff(
217            &target,
218            (xc[xc.len() - 1].0, xc[xc.len() - 1].1, yawc[xc.len() - 1]),
219        );
220        let cost = dc.norm();
221        if cost <= cost_th {
222            break;
223        }
224
225        let j = calc_j(&target, p, h, k0, ds, v, l);
226        let dp = -j
227            .try_inverse()
228            .expect("Jacobian matrix is singular during trajectory optimization")
229            * dc;
230        let alpha = selection_learning_param(p, k0, ds, v, l, &target);
231
232        p += alpha * dp;
233    }
234    (xc, yawc, p)
235}
236
237#[cfg(test)]
238mod tests {
239    use super::*;
240
241    #[test]
242    fn test_optimize_trajectory() {
243        let max_iter = 100;
244        let h = nalgebra::Vector3::new(0.5, 0.02, 0.02);
245        let cost_th = 0.1;
246
247        let k0 = 0.;
248        let l = 1.0; // wheel base
249        let ds = 0.1; // course distance
250        let v = 10.0 / 3.6; // velocity [m/s]
251
252        let init_x = (5., 2., 90. / 180. * std::f64::consts::PI, 0.); // [x, y, yaw, v]
253        let target = State::new(init_x, l);
254
255        let init_p = nalgebra::Vector3::new(6., 0., 0.);
256
257        let (xc, _yawc, _p) =
258            optimize_trajectory(target, init_p, h, k0, ds, v, l, max_iter, cost_th);
259
260        // Trajectory should have been generated
261        assert!(!xc.is_empty());
262    }
263}
rust_robotics_control/model_predictive_trajectory_generator.rs

rust_robotics_control/
model_predictive_trajectory_generator.rs
