lqr 0.1.0

A generic LQR feedback controller
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253

use nalgebra as na;
extern crate rand;
use core::fmt;
use na::allocator::Allocator;
use na::base::DefaultAllocator;
use na::core::dimension::{Dim, DimMin, DimName};
use std::error::Error;

#[derive(Clone, Debug)]
/// Generic LQR optimal feedback controller
///
/// Templated via T,S,C which are numeric type, state and control dimensions
///
/// Riccatti equation solved via
/// https://www.tandfonline.com/doi/abs/10.1080/00207170410001714988
/// https://scicomp.stackexchange.com/questions/30757/discrete-time-algebraic-riccati-equation-dare-solver-in-c
pub struct LQRController<T, S, C>
where
    T: na::RealField,
    S: Dim + DimName + DimMin<S>, // State dimensions
    C: Dim + DimName + DimMin<C>, // Control dimensions
    DefaultAllocator:
        Allocator<T, S, S> + Allocator<T, C, C> + Allocator<T, S, C> + Allocator<T, C, S>,
{
    /// state cost
    pub q: Option<na::OMatrix<T, S, S>>,
    /// control cost
    pub r: Option<na::OMatrix<T, C, C>>,
    /// optimal gain
    pub k: Option<na::OMatrix<T, C, S>>,

    /// the controller also has an i-controller for removing steady state error in y dimension
    /// i-controller coefficient
    ki: T,
    /// accumulating integral error for the i-controller
    integral_error: T,
}

impl<T, S, C> LQRController<T, S, C>
where
    T: na::RealField + Copy,
    S: Dim + DimName + DimMin<S>,
    C: Dim + DimName + DimMin<C>,
    DefaultAllocator:
        Allocator<T, S, S> + Allocator<T, C, C> + Allocator<T, S, C> + Allocator<T, C, S>,
    rand::distributions::Standard: rand::distributions::Distribution<T>,
{
    // Instantiate controller in default mode without the i-controller
    pub fn new() -> Result<LQRController<T, S, C>, &'static str> {
        Ok(LQRController {
            q: None,
            r: None,
            k: None,
            ki: T::zero(),
            integral_error: T::zero(),
        })
    }

    // Initialise the i-controller with its ki coefficients. This enables the controller
    pub fn setup_i_controller(&mut self, ki: T) -> Result<(), Box<dyn Error>> {
        if ki != T::zero() {
            self.ki = ki;
        } else {
            Err("You tried setting ki to 0.0 which is not allowed")?;
        }
        Ok(())
    }

    /// Computes and returns the optimal gain matrix K for the LQR controller
    ///
    /// # Arguments
    ///
    /// * `a` - state matrix of shape SxS
    /// * `b` - control matrix of shape SxC
    /// * `q` - state cost matrix of shape SxS
    /// * `r` - control cost amtrix of shape CxC
    /// * `epsilon` - small value to avoid division by 0; 1e-6 works nicely
    ///
    /// # Returns
    /// optimal feedback gain matrix `k` of shape CxS
    pub fn compute_gain(
        &mut self,
        a: &na::OMatrix<T, S, S>,
        b: &na::OMatrix<T, S, C>,
        q: &na::OMatrix<T, S, S>,
        r: &na::OMatrix<T, C, C>,
        epsilon: T,
    ) -> Result<na::OMatrix<T, C, S>, &'static str> {
        let mut a_k = a.clone(); // copy to here
        let mut g_k = b.clone()
            * r.clone().try_inverse().expect("Couldn't compute inverse")
            * b.clone().transpose();

        let mut h_k_1 = q.clone();
        let mut h_k = na::OMatrix::<T, S, S>::from_fn(|_, _| rand::random());

        while {
            let error = (h_k_1.clone() - h_k).norm() / h_k_1.norm();
            h_k = h_k_1.clone();
            error >= epsilon
        } {
            let temp = (na::OMatrix::<T, S, S>::identity() + &g_k * &h_k)
                .try_inverse()
                .expect("Couldn't compute inverse");
            let a_k_1 = &a_k * &temp * &a_k;
            let g_k_1 = &g_k + &a_k * &temp * &g_k * &a_k.transpose();
            h_k_1 = &h_k + &a_k.transpose() * &h_k * &temp * &a_k;
            a_k = a_k_1;
            g_k = g_k_1;
        }

        // calculate final gain matrix
        self.k =
            Some(r.clone().try_inverse().expect("Couldn't compute inverse") * b.transpose() * h_k);
        return Ok(self.k.clone().unwrap());
    }

    /// Returns the optimal feedback control based on the desired and current state vectors.
    /// This should  be called only after compute_gain() has already been called.
    ///
    /// # Arguments
    ///
    /// * `current_state` - vector of length S
    /// * `desired_state` - vector of length S which we want to get to
    ///
    /// # Returns
    /// The feedback control gains. These are insufficient to control anything and have to be
    /// combined with the feedforward controls. Check examples
    pub fn compute_optimal_controls(
        &mut self,
        current_state: &na::OVector<T, S>,
        desired_state: &na::OVector<T, S>,
    ) -> Result<na::OVector<T, C>, &'static str>
    where
        T: na::RealField,
        DefaultAllocator: Allocator<T, S> + Allocator<T, C> + Allocator<T, C, S>,
    {
        let error = desired_state - current_state;
        let y_error = error[1];
        if y_error.abs() < T::from_f64(1e-2).unwrap() {
            self.integral_error = T::zero();
        } else {
            self.integral_error += y_error * self.ki.clone();
        }

        let mut controls = &self.k.clone().unwrap() * error;
        controls[0] -= self.integral_error.clone();

        Ok(controls)
    }
}

impl<T, S, C> fmt::Display for LQRController<T, S, C>
where
    T: na::RealField,
    S: Dim + DimName + DimMin<S>,
    C: Dim + DimName + DimMin<C>,
    DefaultAllocator:
        Allocator<T, S, S> + Allocator<T, C, C> + Allocator<T, S, C> + Allocator<T, C, S>,
    rand::distributions::Standard: rand::distributions::Distribution<T>,
{
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(
            f,
            "LQR controller
            Q: {:?}
            R: {:?}
            ki: {:}",
            self.q, self.r, self.ki,
        )
    }
}

pub struct ClosestIndexError;

/// Finds the two closest indices to the current pose along with their respective
/// Euclidean errors. The idea is that the lower index is behind the car and the upper
/// index is in front of the car. The opposite is true when driving backwards which
/// is controlled via the `forwards` parameter
/// Return format is ((lower_index, lower_error), (upper_index, upper_error)
pub fn find_closest_indices<T, S, N>(
    current_state: &na::OVector<T, S>,
    trajectory: &na::OMatrix<T, S, N>,
) -> Result<((usize, T), (usize, T)), ClosestIndexError>
where
    T: na::RealField + PartialOrd + Copy,
    S: Dim + DimName + DimMin<S>, // State dimensions
    N: Dim + DimName,             // Trajectory length
    DefaultAllocator: Allocator<T, S>
        + Allocator<T, S, N>
        + Allocator<T, na::Const<1_usize>, N>
        + nalgebra::allocator::Allocator<T, N>,
{
    if trajectory.ncols() < 2 {
        return Err(ClosestIndexError);
    }

    let mut state = Vec::with_capacity(current_state.len());
    for i in current_state.iter() {
        state.push(i.clone());
    }
    let state = vec![state; trajectory.ncols()].concat();

    let current_state_broadcasted = na::OMatrix::<T, S, N>::from_vec(state);
    let errors = trajectory - current_state_broadcasted;
    let errors = errors.map(|x| x.powi(2));
    let errors = errors.row_sum_tr();
    let (idx, error) = errors.argmin();

    // figure out lower and upper indices and their errors
    let (lower, upper) = if idx == 0 {
        ((0, error.sqrt()), (0, error.sqrt()))
    } else if idx == errors.len() - 1 {
        (
            (errors.len() - 1, error.sqrt()),
            (errors.len() - 1, error.sqrt()),
        )
    } else {
        if errors[idx + 1] > errors[idx - 1] {
            ((idx, error.sqrt()), (idx + 1, errors[idx + 1].sqrt()))
        } else {
            ((idx - 1, errors[idx - 1].sqrt()), (idx, error.sqrt()))
        }
    };

    // Sanity check
    if lower.0 != 0 && upper.0 != 0 && lower.0 > upper.0 {
        // Incorrectly detected trajectory indices
        return Err(ClosestIndexError);
    }

    Ok((lower, upper))
}

/// Compute a pose target between two different ones via linear interpolation
pub fn compute_target<T, S, N>(
    trajectory: &na::OMatrix<T, S, N>,
    lower: (usize, T),
    upper: (usize, T),
) -> na::OVector<T, S>
where
    T: na::RealField + Copy,
    S: Dim + DimName + DimMin<S>, // State dimensions
    N: Dim + DimName,             // Trajectory length
    DefaultAllocator: Allocator<T, S> + Allocator<T, S, N>,
{
    trajectory
        .column(lower.0)
        .component_mul(&na::OVector::from_element(upper.1 / (lower.1 + upper.1)))
        + trajectory
            .column(upper.0)
            .component_mul(&na::OVector::from_element(lower.1 / (lower.1 + upper.1)))
}