├── .gitignore
├── .gitmodules
├── CMakeLists.txt
├── LICENSE
├── README.md
├── common
    ├── CMakeLists.txt
    ├── system.h
    └── system_test.cpp
├── ekf
    ├── CMakeLists.txt
    ├── ekf.hpp
    └── ekf_demo.cpp
├── ilqr
    ├── CMakeLists.txt
    ├── ilqr.h
    └── ilqr_demo.cpp
├── mpc
    ├── CMakeLists.txt
    ├── mpc.h
    ├── mpc_demo.cpp
    ├── nmpc.h
    └── nmpc_demo.cpp
└── third_party
    └── CMakeLists.txt


/.gitignore:
--------------------------------------------------------------------------------
1 | build
2 | 


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "third_party/qpOASES"]
2 | 	path = third_party/qpOASES
3 | 	url = git@github.com:kylestach/qpOASES
4 | [submodule "third_party/googletest"]
5 | 	path = third_party/googletest
6 | 	url = git@github.com:google/googletest
7 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | include_directories ("${CMAKE_CURRENT_SOURCE_DIR}")
 2 | cmake_minimum_required (VERSION 2.6)
 3 | 
 4 | find_package (Eigen3 3.3 REQUIRED NO_MODULE)
 5 | 
 6 | enable_testing ()
 7 | 
 8 | add_subdirectory (third_party)
 9 | add_subdirectory (mpc)
10 | add_subdirectory (ilqr)
11 | add_subdirectory (ekf)
12 | add_subdirectory (common)
13 | 
14 | include_directories (${gtest_SOURCE_DIR/include} ${gtest_SOURCE_DIR})
15 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2018 Kyle Stachowicz
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # control-theory-workbench
2 | Implementations of algorithms for path planning, system modelling, optimal control, estimation, and more. Intended for academic and educational use.
3 | 


--------------------------------------------------------------------------------
/common/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_executable (SystemTest system_test.cpp)
2 | target_link_libraries (SystemTest gtest gtest_main Eigen3::Eigen)
3 | add_test (SystemTest SystemTest)
4 | 


--------------------------------------------------------------------------------
/common/system.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "eigen3/Eigen/Core"
  4 | #include <vector>
  5 | 
  6 | template<int N = Eigen::Dynamic, int M = N>
  7 | using Matrix = Eigen::Matrix<double, N, M, M == 1 ? Eigen::ColMajor : Eigen::RowMajor>;
  8 | 
  9 | template<int N = Eigen::Dynamic>
 10 | using Vector = Eigen::Matrix<double, N, 1>;
 11 | 
 12 | template<int N, int M, typename T>
 13 | struct Discretized {
 14 |     double dt;
 15 |     T system;
 16 | 
 17 |     Vector<N> operator()(Vector<N> x, Vector<M> u, double t) {
 18 |         Vector<N> k1 = dt * system(x, u, t);
 19 |         Vector<N> k2 = dt * system(Vector<N>(x + k1 / 2), u, t + dt / 2);
 20 |         Vector<N> k3 = dt * system(Vector<N>(x + k2 / 2), u, t + dt / 2);
 21 |         Vector<N> k4 = dt * system(Vector<N>(x + k3), u, t + dt);
 22 |         return x + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
 23 |     }
 24 | };
 25 | 
 26 | template<int N, int M, typename T>
 27 | Discretized<N, M, T> discretize(T sys, double dt) {
 28 |     return {dt, sys};
 29 | }
 30 | 
 31 | template<int N, int M, int P = N>
 32 | struct Linearized {
 33 |     Matrix<P, N> A;
 34 |     Matrix<P, M> B;
 35 |     Matrix<P, 1> c;
 36 | };
 37 | 
 38 | template<int N, int M, typename System, int P = N>
 39 | Linearized<N, M, P> linearize(System sys, Vector<N> x, Vector<M> u, int t) {
 40 |     Linearized<N, M, P> result;
 41 | 
 42 |     Vector<P> value = sys(x, u, t);
 43 | 
 44 |     const double epsx = 1e-8 * std::max(1e-2, x.norm());
 45 | 
 46 |     for (int i = 0; i < N; i++) {
 47 |         Vector<N> dx = Vector<N>::Zero();
 48 |         dx(i) = epsx;
 49 |         result.A.template block<P, 1>(0, i) = (sys(x + dx, u, t) - value) / epsx;
 50 |     }
 51 | 
 52 |     const double epsu = 1e-8 * std::max(1e-2, u.norm());
 53 |     for (int i = 0; i < M; i++) {
 54 |         Vector<M> du = Vector<M>::Zero();
 55 |         du(i) = epsu;
 56 |         result.B.template block<P, 1>(0, i) = (sys(x, u + du, t) - value) / epsu;
 57 |     }
 58 | 
 59 |     result.c = value - result.A * x - result.B * u;
 60 | 
 61 |     return result;
 62 | }
 63 | 
 64 | template<int N, int L>
 65 | struct LinearSensor {
 66 |     Matrix<L, N> C;
 67 |     Vector<L> c0;
 68 | };
 69 | 
 70 | template<int M, int N, int L, typename System>
 71 | LinearSensor<N, L> linearize_measurement(System sys, Vector<N> x, Vector<M> u, int t) {
 72 |     LinearSensor<N, L> result;
 73 | 
 74 |     Vector<L> value = sys(x, u, t);
 75 | 
 76 |     const double epsx = 1e-8 * std::max(1e-2, x.norm());
 77 | 
 78 |     for (int i = 0; i < N; i++) {
 79 |         Vector<N> dx = Vector<N>::Zero();
 80 |         dx(i) = epsx;
 81 |         result.C.template block<L, 1>(0, i) = (sys(x + dx, u, t) - value) / epsx;
 82 |     }
 83 | 
 84 |     result.c = value - result.C * x;
 85 | 
 86 |     return result;
 87 | }
 88 | 
 89 | template<int N, int M>
 90 | struct Quadratized {
 91 |     Matrix<N> Q;
 92 |     Matrix<N, M> P;
 93 |     Matrix<M> R;
 94 |     Matrix<N, 1> q;
 95 |     Matrix<M, 1> r;
 96 |     double c;
 97 | };
 98 | 
 99 | template<int N, int M, typename Cost>
100 | Quadratized<N, M> quadratize(Cost cost, Vector<N> x, Vector<M> u, int t) {
101 |     Quadratized<N, M> result;
102 | 
103 |     double value = cost(x, u, t);
104 | 
105 |     constexpr double eps = 1e-5;
106 | 
107 |     for (int i = 0; i < N; i++) {
108 |         Vector<N> dx1 = Vector<N>::Zero();
109 |         dx1(i) = eps;
110 |         double dc_dx1 = (cost(x + dx1, u, t) - value) / eps;
111 |         for (int j = i; j < N; j++) {
112 |             Vector<N> dx2 = Vector<N>::Zero();
113 |             dx2(j) = eps;
114 |             double dc_dx1_stepped = (cost(x + dx1 + dx2, u, t) - cost(x + dx2, u, t)) / eps;
115 |             result.Q(i, j) = result.Q(j, i) = (dc_dx1_stepped - dc_dx1) / eps;
116 |         }
117 |         for (int j = 0; j < M; j++) {
118 |             Vector<M> du1 = Vector<M>::Zero();
119 |             du1(j) = eps;
120 |             double dc_dx1_stepped = (cost(x + dx1, u + du1, t) - cost(x, u + du1, t)) / eps;
121 |             result.P(i, j) = (dc_dx1_stepped - dc_dx1) / eps;
122 |         }
123 |         result.q(i) = dc_dx1;
124 |     }
125 | 
126 |     for (int i = 0; i < M; i++) {
127 |         Vector<M> du1 = Vector<M>::Zero();
128 |         du1(i) = eps;
129 |         double dc_du1 = (cost(x, u + du1, t) - value) / eps;
130 |         for (int j = i; j < M; j++) {
131 |             Vector<M> du2 = Vector<M>::Zero();
132 |             du2(j) = eps;
133 |             double dc_du1_stepped = (cost(x, u + du1 + du2, t) - cost(x, u + du2, t)) / eps;
134 |             result.R(i, j) = result.R(j, i) = (dc_du1_stepped - dc_du1) / eps;
135 |         }
136 |         result.r(i) = dc_du1;
137 |     }
138 | 
139 |     result.q -= result.Q * x + result.P * u;
140 |     result.r -= result.R * u + x.transpose() * result.P;
141 | 
142 |     result.c = value - (
143 |             x.dot(0.5 * result.Q * x + result.P * u + result.q) +
144 |             u.dot(0.5 * result.R * u + result.r));
145 | 
146 |     return result;
147 | }
148 | 
149 | template<int N>
150 | struct QuadratizedFinal {
151 |     Matrix<N> Q;
152 |     Matrix<N, 1> q;
153 |     double c;
154 | };
155 | 
156 | template<int N, typename CostFinal>
157 | QuadratizedFinal<N> quadratize_final(CostFinal cost, Vector<N> x) {
158 |     QuadratizedFinal<N> result;
159 | 
160 |     Quadratized<N, 1> quad_full = quadratize([&cost](Vector<N> x, Vector<1>, int t) {
161 |         return cost(x);
162 |     }, x, Vector<1>(0), 0);
163 | 
164 |     result.Q = quad_full.Q;
165 |     result.q = quad_full.q;
166 |     result.c = quad_full.c;
167 |     return result;
168 | }
169 | 
170 | template<int N, int M>
171 | struct Policy {
172 |     std::vector<Matrix<M, N>> K;
173 |     std::vector<Matrix<M, 1>> k;
174 | };
175 | 
176 | template<int N, int L>
177 | struct GaussianNoise {
178 |     Matrix<N> Q;
179 |     Matrix<L> R;
180 | };
181 | 


--------------------------------------------------------------------------------
/common/system_test.cpp:
--------------------------------------------------------------------------------
  1 | #include "system.h"
  2 | #include "gtest/gtest.h"
  3 | #include <unsupported/Eigen/MatrixFunctions>
  4 | 
  5 | TEST(Discretized, SimpleLinearDiscretize) {
  6 |     auto continuous = [](Vector<2> x, Vector<1> u, double dt) {
  7 |         return Vector<2>(x(1), u(0) - x(0));
  8 |     };
  9 |     double dt = 0.01;
 10 |     auto discretized = discretize<2, 1>(continuous, dt);
 11 | 
 12 |     Vector<2> x0(1.0, 1.0);
 13 |     double T = 1.0;
 14 |     Matrix<2> Ad = ((Matrix<2>() << 0, 1, -1, 0).finished() * T).exp();
 15 |     Vector<2> x1 = Ad * x0;
 16 | 
 17 |     Vector<2> x = x0;
 18 |     for (double t = 0; t < T; t += dt) {
 19 |         x = discretized(x, Vector<1>(0), t);
 20 |     }
 21 | 
 22 |     EXPECT_NEAR(x(0), x1(0), 1e-5);
 23 |     EXPECT_NEAR(x(1), x1(1), 1e-5);
 24 | }
 25 | 
 26 | TEST(Discretized, NonlinearDiscretize) {
 27 |     auto continuous = [](Vector<2> x, Vector<1> u, double dt) {
 28 |         return Vector<2>(x(1), u(0) - std::sin(x(0)));
 29 |     };
 30 |     double dt = 0.01;
 31 |     auto discretized = discretize<2, 1>(continuous, dt);
 32 | 
 33 |     Vector<2> x0(-3.0, 0.0);
 34 | 
 35 |     double T = 10.0;
 36 | 
 37 |     Vector<2> x = x0;
 38 |     double xmax0 = x0(0);
 39 |     for (double t = 0; t < T; t += dt) {
 40 |         x = discretized(x, Vector<1>(0), t);
 41 | 
 42 |         xmax0 = std::max(x(0), xmax0);
 43 |     }
 44 | 
 45 |     EXPECT_NEAR(xmax0, 3.0, 1e-5);
 46 | }
 47 | 
 48 | TEST(Discretized, SimpleLinearDiscretizeReverse) {
 49 |     auto continuous = [](Vector<2> x, Vector<1> u, double dt) {
 50 |         return Vector<2>(x(1), u(0) - x(0));
 51 |     };
 52 |     double dt = -0.01;
 53 |     auto discretized = discretize<2, 1>(continuous, dt);
 54 | 
 55 |     Vector<2> x0(1.0, 1.0);
 56 |     double T = -1.0;
 57 |     Matrix<2> Ad = ((Matrix<2>() << 0, 1, -1, 0).finished() * T).exp();
 58 |     Vector<2> x1 = Ad * x0;
 59 | 
 60 |     Vector<2> x = x0;
 61 |     for (double t = 0; t > T; t += dt) {
 62 |         x = discretized(x, Vector<1>(0), t);
 63 |     }
 64 | 
 65 |     EXPECT_NEAR(x(0), x1(0), 1e-5);
 66 |     EXPECT_NEAR(x(1), x1(1), 1e-5);
 67 | }
 68 | 
 69 | TEST(Linearized, SimpleLinearization) {
 70 |     auto nonlinear = [](Vector<2> x, Vector<1> u, double dt) {
 71 |         return Vector<2>(x(1), u(0) - std::sin(x(0)));
 72 |     };
 73 |     Vector<2> x(0, 0);
 74 |     Vector<1> u(0);
 75 |     auto linear = linearize<2, 1>(nonlinear, x, u, 0.0);
 76 |     EXPECT_NEAR((linear.A - (Matrix<2>() << 0, 1, -1, 0).finished()).lpNorm<Eigen::Infinity>(), 0, 1e-5);
 77 |     EXPECT_NEAR((linear.B - (Matrix<2, 1>() << 0, 1).finished()).lpNorm<Eigen::Infinity>(), 0, 1e-5);
 78 |     EXPECT_NEAR(linear.c.lpNorm<Eigen::Infinity>(), 0, 1e-5);
 79 | 
 80 |     x(0) = 1;
 81 |     linear = linearize<2, 1>(nonlinear, x, u, 0.0);
 82 |     EXPECT_NEAR((linear.A - (Matrix<2>() << 0, 1, -std::cos(x(0)), 0).finished()).lpNorm<Eigen::Infinity>(), 0, 1e-5);
 83 |     EXPECT_NEAR((linear.B - (Matrix<2, 1>() << 0, 1).finished()).lpNorm<Eigen::Infinity>(), 0, 1e-5);
 84 |     EXPECT_NEAR((linear.c - (Vector<2>() << 0, x(0) * std::cos(x(0)) - std::sin(x(0))).finished()).lpNorm<Eigen::Infinity>(), 0, 1e-5);
 85 | }
 86 | 
 87 | TEST(Linearized, GeneralLinearization) {
 88 |     auto nonlinear = [](Vector<2> x, Vector<1> u, double dt) {
 89 |         return Vector<2>(std::cos(x(1)), std::cos(u(0)) - std::tanh(x(1) - x(0)));
 90 |     };
 91 | 
 92 |     constexpr int kNumSamples = 6;
 93 |     constexpr double kSampleOffset = kNumSamples / 2;
 94 |     for (int i = 0; i <= kNumSamples; i++) {
 95 |         for (int j = 0; j <= kNumSamples; j++) {
 96 |             for (int k = 0; k <= kNumSamples; k++) {
 97 |                 Vector<2> x0((-kSampleOffset + i) / kSampleOffset, (-kSampleOffset + j) / kSampleOffset);
 98 |                 Vector<1> u0((-kSampleOffset + k) / kSampleOffset);
 99 |                 auto linearized = linearize<2, 1>(nonlinear, x0, u0, 0.0);
100 |                 for (int l = 0; l <= 4; l++) {
101 |                     for (int m = 0; m <= 4; m++) {
102 |                         for (int n = 0; n <= 4; n++) {
103 |                             Vector<2> dx((-2 + l) / 10.0, (-2 + m) / 10.0);
104 |                             Vector<1> du((-2 + n) / 10.0);
105 |                             Vector<2> x = x0 + dx;
106 |                             Vector<1> u = u0 + du;
107 |                             Vector<2> xhat = linearized.A * x + linearized.B * u + linearized.c;
108 |                             EXPECT_LT((xhat - nonlinear(x, u, 0)).lpNorm<Eigen::Infinity>() / (dx.norm() + du.norm() + 1e-5), 0.25);
109 |                         }
110 |                     }
111 |                 }
112 |             }
113 |         }
114 |     }
115 | }
116 | 
117 | TEST(Quadratized, SimpleQuadratization) {
118 |     auto nonquadratic = [](Vector<2> x, Vector<1> u, double dt) {
119 |         return std::exp(x(0) + x(0) * x(1) + x(1) * u(0));
120 |     };
121 |     Vector<2> x(0, 0);
122 |     Vector<1> u(0);
123 |     auto quadratized = quadratize(nonquadratic, x, u, 0);
124 |     EXPECT_NEAR(quadratized.Q(0, 0), 1, 1e-3);
125 |     EXPECT_NEAR(quadratized.Q(1, 0), 1, 1e-3);
126 |     EXPECT_NEAR(quadratized.Q(0, 1), 1, 1e-3);
127 |     EXPECT_NEAR(quadratized.q(0, 0), 1, 1e-3);
128 |     EXPECT_NEAR(quadratized.P(1, 0), 1, 1e-3);
129 |     EXPECT_NEAR(quadratized.q(1, 0), 0, 1e-3);
130 | }
131 | 
132 | TEST(Quadratized, GeneralQuadratization) {
133 |     auto nonquadratic = [](Vector<2> x, Vector<1> u, double dt) {
134 |         return std::exp(x(0) * x(0)) + u(0) * u(0);
135 |     };
136 | 
137 |     Vector<2> x(1, 1);
138 |     Vector<1> u(1);
139 |     auto quadratic = quadratize<2, 1>(nonquadratic, x, u, 0);
140 | 
141 |     constexpr int kNumSamples = 6;
142 |     constexpr double kSampleOffset = kNumSamples / 2;
143 |     for (int i = 0; i <= kNumSamples; i++) {
144 |         for (int j = 0; j <= kNumSamples; j++) {
145 |             for (int k = 0; k <= kNumSamples; k++) {
146 |                 Vector<2> x0((-kSampleOffset + i) / kSampleOffset, (-kSampleOffset + j) / kSampleOffset);
147 |                 Vector<1> u0((-kSampleOffset + k) / kSampleOffset);
148 |                 auto quadratized = quadratize<2, 1>(nonquadratic, x0, u0, 0.0);
149 |                 double c0 = nonquadratic(x0, u0, 0.0);
150 |                 for (int l = 0; l <= 2; l++) {
151 |                     for (int m = 0; m <= 2; m++) {
152 |                         for (int n = 0; n <= 4; n++) {
153 |                             Vector<2> dx((-2 + l) / 10.0, (-2 + m) / 10.0);
154 |                             Vector<1> du((-2 + n) / 10.0);
155 |                             Vector<2> x = x0 + dx;
156 |                             Vector<1> u = u0 + du;
157 |                             double chat = x.dot(0.5 * quadratized.Q * x + quadratized.P * u + quadratized.q) + u.dot(0.5 * quadratized.R * u + quadratized.r) + quadratized.c;
158 |                             EXPECT_NEAR((chat - nonquadratic(x, u, 0)) / (1e-5 + std::abs(nonquadratic(x, u, 0) - c0)), 0,  15 * (1e-3 + x.squaredNorm() + u.squaredNorm()));
159 |                         }
160 |                     }
161 |                 }
162 |             }
163 |         }
164 |     }
165 | }
166 | 


--------------------------------------------------------------------------------
/ekf/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_library (ekf INTERFACE)
2 | 
3 | add_executable (ekf_demo ekf_demo.cpp)
4 | target_link_libraries (ekf_demo ekf)
5 | 


--------------------------------------------------------------------------------
/ekf/ekf.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "common/system.h"
 4 | 
 5 | template<int M, int N, int L, typename System>
 6 | class ExtendedKalmanFilter {
 7 | public:
 8 |     ExtendedKalmanFilter(System sys, GaussianNoise<N, L> noise, Vector<N> x0, Matrix<N> P0)
 9 |         : sys(sys), noise(noise), x(x0), u_prev(Vector<M>::Zero()), P(P0) {}
10 | 
11 |     void predict(Vector<M> u, size_t t = 0) {
12 |         auto l = linearize<N, M, System>(sys, x, u, t);
13 |         x = l.A * x + l.B * u + l.c;
14 |         P = l.A * P * l.A.transpose() + noise.Q;
15 |     }
16 | 
17 |     void update(Vector<L> y, size_t t = 0) {
18 |         auto ls = linearize<N, M, System>(sys, x, u_prev, t);
19 |         Vector<L> y_err = y - sys.measure(x, u_prev, t);
20 |         Matrix<L> S = noise.R + ls.C * P * ls.C.transpose();
21 |         Matrix<N, L> K = P * ls.C.transpose() * S.inverse();
22 |         x += K * y_err;
23 |         Matrix<N> IsubKC = Matrix<N>::Identity() - K * ls.C;
24 |         P = IsubKC * P * IsubKC.transpose() + K * noise.R * K.T;
25 |     }
26 | 
27 | private:
28 |     System sys;
29 |     GaussianNoise<N, L> noise;
30 |     Vector<N> x;
31 |     Vector<M> u_prev;
32 |     Matrix<N> P;
33 | };
34 | 


--------------------------------------------------------------------------------
/ekf/ekf_demo.cpp:
--------------------------------------------------------------------------------
 1 | #include "ekf.hpp"
 2 | #include "eigen3/Eigen/Dense"
 3 | 
 4 | constexpr double dt = 0.1;
 5 | 
 6 | struct System {
 7 |     Vector<2> operator()(Vector<2> x, Vector<1> u, int) {
 8 |         return Vector<2>(x(1), u(0) - 9.81 * std::sin(x(0)));
 9 |     }
10 | 
11 |     Vector<1> measure(Vector<2> x, Vector<1> = Vector<1>::Zero(), int = 0) {
12 |         return Vector<1>(x(0));
13 |     }
14 | };
15 | 
16 | int main() {
17 |     System sys;
18 |     GaussianNoise<2, 1> noise {Matrix<2>::Identity(), Matrix<1>::Identity()};
19 |     ExtendedKalmanFilter<1, 2, 1, System>(
20 |             sys, noise, Vector<2>(0, 0), Matrix<2>::Identity());
21 | }
22 | 


--------------------------------------------------------------------------------
/ilqr/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_library (ilqr INTERFACE)
2 | 
3 | add_executable (ilqr_demo ilqr_demo.cpp)
4 | target_link_libraries (ilqr_demo ilqr)
5 | 


--------------------------------------------------------------------------------
/ilqr/ilqr.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "common/system.h"
 4 | #include "eigen3/Eigen/Dense"
 5 | 
 6 | template<int N, int M>
 7 | void lqr_step(
 8 |         Linearized<N, M> l,
 9 |         Quadratized<N, M> q,
10 |         Matrix<N, N> S_tn,
11 |         Matrix<N, 1> s_tn,
12 |         Matrix<N, N>& S_t,
13 |         Matrix<N, 1>& s_t,
14 |         Matrix<M, N>& K_t,
15 |         Matrix<M, 1>& k_t) {
16 |     Matrix<N, M> C = q.P + l.A.transpose() * S_tn * l.B;
17 |     Matrix<N, N> D = q.Q + l.A.transpose() * S_tn * l.A;
18 |     Matrix<M, M> E = q.R + l.B.transpose() * S_tn * l.B;
19 |     Matrix<N, 1> d = q.q + l.A.transpose() * (s_tn + S_tn * l.c);
20 |     Matrix<M, 1> e = q.r + l.B.transpose() * (s_tn + S_tn * l.c);
21 | 
22 |     Matrix<M, M> E_inv = E.inverse();
23 |     S_t = D - C * E_inv * C.transpose();
24 |     s_t = d - C * E_inv * e;
25 |     K_t = E_inv * C.transpose();
26 |     k_t = E_inv * e;
27 | }
28 | 
29 | template<int N, int M, typename System, typename Cost, typename CostFinal>
30 | Policy<N, M> ilqr(System sys, Cost cost, CostFinal cost_final, Vector<N> x0, Vector<N> xf, int L) {
31 |     std::vector<Matrix<N>> S(L + 1);
32 |     std::vector<Matrix<N, 1>> s(L + 1);
33 | 
34 |     std::vector<Matrix<M, N>> K(L);
35 |     std::vector<Matrix<M, 1>> k(L);
36 | 
37 |     std::vector<Matrix<N, 1>> x(L + 1);
38 | 
39 |     constexpr double dt = 0.01;
40 |     Discretized<N, M, System> g{dt, sys};
41 | 
42 |     for (int i = 0; i <= L; i++) {
43 |         S[i] = Matrix<N>::Zero();
44 |         s[i] = Matrix<N, 1>::Zero();
45 |         double t = ((double) i) / L;
46 |         x[i] = x0 + t * (xf - x0);
47 |         if (i < L) {
48 |             K[i] = Matrix<M, N>::Zero();
49 |             k[i] = Matrix<M, 1>::Zero();
50 |         }
51 |     }
52 | 
53 |     double last_cost = std::numeric_limits<double>::infinity();
54 |     constexpr int num_iterations = 100;
55 |     for (int i = 0; i < num_iterations; i++) {
56 |         QuadratizedFinal<N> qf = quadratize_final(cost_final, x[L]);
57 |         S[L] = qf.Q;
58 |         s[L] = qf.q;
59 | 
60 |         // Backwards pass: calculate S and K
61 |         for (int t = L - 1; t >= 0; t--) {
62 |             Matrix<M, 1> u = -(K[t] * x[t] + k[t]);
63 |             Linearized<N, M> l = linearize(g, x[t], u, t);
64 |             Quadratized<N, M> q = quadratize(cost, x[t], u, t);
65 |             lqr_step(l, q, S[t + 1], s[t + 1], S[t], s[t], K[t], k[t]);
66 |         }
67 | 
68 |         // Forwards pass: calculate x
69 |         double total_cost = 0;
70 |         for (int t = 0; t < L; t++) {
71 |             Vector<M> u = -(K[t] * x[t] + k[t]);
72 |             x[t + 1] = g(x[t], u, t);
73 |             total_cost += cost(x[t], u, t);
74 |         }
75 |         total_cost += cost_final(x[L]);
76 | 
77 |         double delta_cost = last_cost - total_cost;
78 |         double relative_cost = delta_cost / total_cost;
79 |         if (std::abs(last_cost - total_cost) / total_cost < 1e-4) {
80 |             break;
81 |         }
82 |         last_cost = total_cost;
83 |     }
84 | 
85 |     return {std::move(K), std::move(k)};
86 | }
87 | 


--------------------------------------------------------------------------------
/ilqr/ilqr_demo.cpp:
--------------------------------------------------------------------------------
 1 | #include "ilqr.h"
 2 | 
 3 | #include <vector>
 4 | #include <limits>
 5 | #include <iostream>
 6 | 
 7 | struct ContinuousPendulum {
 8 |     Vector<2> operator()(Vector<2> x, Vector<1> u, int t) {
 9 |         return Vector<2>(x(1), u(0) - 9.81 * std::sin(x(0)));
10 |     }
11 | };
12 | 
13 | struct QuadraticCost {
14 |     double operator()(Vector<2> x, Vector<1> u, int t) {
15 |         return 1 * (u(0) * u(0));
16 |     }
17 | };
18 | 
19 | struct ExponentialFinalCost {
20 |     double operator()(Vector<2> x) {
21 |         return 4000 * (x(0) - M_PI) * (x(0) - M_PI) + 10000 * x(1) * x(1);
22 |     }
23 | };
24 | 
25 | int main() {
26 |     ContinuousPendulum pendulum;
27 |     QuadraticCost cost;
28 |     ExponentialFinalCost cost_final;
29 |     Discretized<2, 1, ContinuousPendulum> g{.01, pendulum};
30 | 
31 |     Vector<2> x(0, 0);
32 | 
33 |     int N = 600;
34 |     Policy<2, 1> p = ilqr<2, 1, ContinuousPendulum, QuadraticCost, ExponentialFinalCost>(pendulum, cost, cost_final, x, Vector<2>(M_PI, 0), N);
35 |     for (int t = 0; t < N; t++) {
36 |         Vector<1> u = -(p.K[t] * x + p.k[t]);
37 |         std::cout << x(0) << ", " << x(1) << ", " << u(0) << std::endl;
38 |         x = g(x, u, t);
39 |     }
40 | }
41 | 


--------------------------------------------------------------------------------
/mpc/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | include_directories ("${PROJECT_SOURCE_DIR}/third_party/qpOASES/include")
 2 | 
 3 | add_library (mpc INTERFACE)
 4 | target_link_libraries (mpc INTERFACE qpOASES)
 5 | 
 6 | add_library (nmpc INTERFACE)
 7 | target_link_libraries (nmpc INTERFACE qpOASES)
 8 | 
 9 | add_executable (mpc_demo mpc_demo.cpp)
10 | target_link_libraries (mpc_demo mpc)
11 | 
12 | add_executable (nmpc_demo nmpc_demo.cpp)
13 | target_link_libraries (nmpc_demo nmpc)
14 | 


--------------------------------------------------------------------------------
/mpc/mpc.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <qpOASES.hpp>
  4 | #include "common/system.h"
  5 | 
  6 | template<int N, int M>
  7 | class ModelPredictiveController {
  8 | public:
  9 |     ModelPredictiveController(Matrix<N> A, Matrix<N, M> B, Vector<M> lb, Vector<M> ub, Matrix<N> Q, Matrix<M> R, int L)
 10 |             : problem(M * (L - 1)), A(A), B(B), Q(Q), R(R), L(L) {
 11 |         // Minimize sum(i = 0 to L - 1)(1/2[x^T Q x + u^T R u]) + x^T Qf x
 12 |         // Subject to x(i + 1) = Ax(i) + Bu(i)
 13 |         //
 14 |         // Make a big transition matrix:
 15 |         // x(0) = x(0)
 16 |         // x(1) = Ax(0) + Bu(0)
 17 |         // x(2) = A^2x(0) + ABu(0) + Bu(1)
 18 |         // x(3) = A^3x(0) + A^2Bu(0) + ABu(1) + Bu(2)
 19 |         // ...
 20 |         // x(L) = A^Lx(0) + A^(L-1)Bu(0) + A^(L-2)Bu(1)
 21 |         // So we have:
 22 |         // [x(0)]   = [ 0   0  ...  0  0 ] [ u(0) ]  + [ x(0)    ]
 23 |         // [x(1)]   = [ B   0  ...  0  0 ] [ u(1) ]  + [ Ax(0)   ]
 24 |         // [x(2)]   = [ AB  B  ...  0  0 ] [ u(2) ]  + [ A^2x(0) ]
 25 |         // [x(3)]   = [A^2B AB ...  0  0 ] [ u(2) ]  + [ A^3x(0) ]
 26 |         //   .      .   .   .  ...  .  .     .       .    .
 27 |         // [x(L)]   = [A^LB .  ...  B  0 ] [ u(L) ]  + [ A^Lx(0) ]
 28 |         // We can ignore the first row (you can't affect x0) and the last column (the last input does nothing)
 29 |         A_aug = Matrix<>::Zero(N * (L - 1), N * (L - 1));
 30 |         B_aug = Matrix<>::Zero(N * (L - 1), M * (L - 1));
 31 |         Q_aug = Matrix<>::Zero(N * (L - 1), N * (L - 1));
 32 |         R_aug = Matrix<>::Zero(M * (L - 1), M * (L - 1));
 33 |         lb_aug = Vector<>::Zero(M * (L - 1));
 34 |         ub_aug = Vector<>::Zero(M * (L - 1));
 35 | 
 36 |         Matrix<> A_i = Matrix<>::Identity(N, N);
 37 |         for (int i = 0; i < (L - 1); i++) {
 38 |             A_aug.template block<N, N>(N * i, N * i) = A_i;
 39 |             Q_aug.template block<N, N>(N * i, N * i) = Q;
 40 |             R_aug.template block<M, M>(M * i, M * i) = R;
 41 |             lb_aug.template block<M, 1>(M * i, 0) = lb;
 42 |             ub_aug.template block<M, 1>(M * i, 0) = ub;
 43 | 
 44 |             for (int j = 0; i + j < (L - 1); j++) {
 45 |                 B_aug.template block<N, M>(N * (i + j), M * j) = A_i * B;
 46 |             }
 47 |             A_i *= A;
 48 |         }
 49 | 
 50 |         cost_quadratic = R_aug + B_aug.transpose() * Q_aug * B_aug;
 51 | 
 52 |         Matrix<> cost_linear = Matrix<>::Zero(1, M * (L - 1));
 53 | 
 54 |         qpOASES::Options options;
 55 |         options.printLevel = qpOASES::PL_NONE;
 56 |         problem.setOptions(options);
 57 | 
 58 |         int nWSR = 1000000;
 59 |         problem.init(cost_quadratic.data(), cost_linear.data(), lb_aug.data(), ub_aug.data(), nWSR);
 60 |     }
 61 | 
 62 |     Vector<M> control(Vector<N> x0, Vector<N> r) {
 63 |         Vector<> Ax_aug = Vector<>::Zero(N * (L - 1));
 64 |         Vector<> r_aug = Vector<>::Zero(N * (L - 1));
 65 | 
 66 |         Matrix<> A_i = Matrix<>::Identity(N, N);
 67 |         for (int i = 0; i < (L - 1); i++) {
 68 |             Ax_aug.template block<N, 1>(N * i, 0) = A_i * x0;
 69 |             r_aug.template block<N, 1>(N * i, 0) = r;
 70 |             A_i *= A;
 71 |         }
 72 | 
 73 |         // cost = 1/2[(x - r)^T Q (x - r) + u^T R u]
 74 |         // x = Ax0 + Bu
 75 |         // cost = 1/2[x0^T A^T Q A x0 - 2r^T Q (Ax0 + Bu) + 2x0^T A^T Q B u + u^T (R + B^T Q B) u + r^T Q r]
 76 |         // cost = 1/2 u^T [R + B^T Q B] u + [x0^T A^T Q B - r^T Q B] u
 77 |         Matrix<> cost_linear = Ax_aug.transpose() * Q_aug * B_aug - r_aug.transpose() * Q_aug * B_aug;
 78 | 
 79 |         int nWSR = 100000000;
 80 |         double cputime = 100000000;
 81 | 
 82 |         Vector<> u_min = Vector<>::Zero(M * (L - 1));
 83 |         problem.hotstart(cost_linear.data(), lb_aug.data(), ub_aug.data(), nWSR, &cputime);
 84 |         problem.getPrimalSolution(u_min.data());
 85 | 
 86 |         return u_min.template block<M, 1>(0, 0);
 87 |     }
 88 | 
 89 | protected:
 90 |     qpOASES::QProblemB problem;
 91 |     Matrix<N, N> A;
 92 |     Matrix<N, M> B;
 93 |     Vector<> lb_aug, ub_aug;
 94 |     Matrix<N, N> Q;
 95 |     Matrix<M, M> R;
 96 |     int L;
 97 | 
 98 |     Matrix<> A_aug;
 99 |     Matrix<> B_aug;
100 |     Matrix<> Q_aug;
101 |     Matrix<> R_aug;
102 |     Matrix<> cost_quadratic;
103 | };
104 | 


--------------------------------------------------------------------------------
/mpc/mpc_demo.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include "mpc.h"
 3 | 
 4 | int main() {
 5 |     Matrix<2> A;
 6 |     A << 1.0, 0.01,
 7 |          0.00, 0.99;
 8 |     Matrix<2, 1> B;
 9 |     B << 0.0,
10 |          0.01;
11 |     Matrix<2> Q = Matrix<2>::Identity();
12 |     Q(1, 1) = 0;
13 |     Matrix<1> R = Matrix<1>::Zero();
14 | 
15 |     Vector<2> x(0, 1);
16 | 
17 |     ModelPredictiveController<2, 1> mpc(A, B, Vector<1>(-1), Vector<1>(1), Q, R, 100);
18 |     for (int i = 0; i < 1000; i++) {
19 |         auto u = mpc.control(x, Vector<2>(0.0, 0.0));
20 |         std::cout << x(0) << ", " << x(1) << ", " << u(0) << std::endl;
21 |         x = A * x + B * u;
22 |     }
23 | }
24 | 


--------------------------------------------------------------------------------
/mpc/nmpc.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <qpOASES.hpp>
  4 | #include "common/system.h"
  5 | 
  6 | template<int N, int M, int C, typename System, typename Cost, typename CostFinal, typename Constraint>
  7 | class NonlinearModelPredictiveController {
  8 | public:
  9 |     NonlinearModelPredictiveController(
 10 |             System sys, Cost cost, CostFinal cost_final, Constraint constraint, Vector<C> lb_constraint, Vector<C> ub_constraint, Vector<M> lb_input, Vector<M> ub_input, int L)
 11 |             : problem(M * (L - 1), C * (L - 1)),
 12 |               system(sys),
 13 |               constraint(constraint),
 14 |               cost(cost),
 15 |               cost_final(cost_final),
 16 |               lb_constraint(lb_constraint),
 17 |               ub_constraint(ub_constraint),
 18 |               lb_input(lb_input),
 19 |               ub_input(ub_input),
 20 |               L(L) {
 21 |         // Minimize sum(i = 0 to L - 1)(1/2[x^T Q x + u^T R u]) + x^T Qf x
 22 |         // Subject to x(i + 1) = Ax(i) + Bu(i)
 23 |         //
 24 |         // Make a big transition matrix:
 25 |         // x(0) = x(0)
 26 |         // x(1) = [B(0)u(0)] + [A(0)x(0) + c(0)]
 27 |         // x(2) = [A(1)B(0)u(0) + B(1)u(1)] + [A(1)A(0)x(0) + c(1) + A(1)c(0) = A(1)x(0) + c(1)]
 28 |         // x(3) = [A(2)A(1)B(0)u(0) + A(2)B(1)u(1)] + [A(2)A(1)A(0)x(0) + A(2)c(1) + A(2)A(1)c(0) = A(2)x(1) + c(2)]
 29 |         // ...
 30 |         // So we have (omitting the first row and the last column):
 31 |         // [x(0)]   = [ 0   0  ...  0  0 ] [ u(0) ]  + [ x(0)    ]
 32 |         // [x(1)]   = [ B   0  ...  0  0 ] [ u(1) ]  + [ Ax(0) + Ic ]
 33 |         // [x(2)]   = [ AB  B  ...  0  0 ] [ u(2) ]  + [ A^2x(0) + (A + I)c]
 34 |         // [x(3)]   = [A^2B AB ...  0  0 ] [ u(2) ]  + [ A^3x(0) + (A^2 + A + I)c]
 35 |         //   .      .   .   .  ...  .  .     .       .    .
 36 |         // [x(L)]   = [A^LB .  ...  B  0 ] [ u(L) ]  + [ A^Lx(0) + (A^(L-1) + ... + I)c]
 37 |         // We can ignore the first row (you can't affect x0) and the last column (the last input does nothing)
 38 |         //
 39 |         // Just like in regular linear MPC, we need to make a big transition matrix. However, at each point in
 40 |         // time we need to linearize around some x. We'll use the zero vector to initialize.
 41 |         //
 42 |         // We also have a constraint matrix. Our constraint is: lb_constraint <= H_u * u + H_x * x <= ub_constraint:
 43 |         // H_u * u + H_x * x + c = H_u * u + H_x * (x_aug + B_aug * u) + c = (H_u + H_x * B_aug)u + (H_x * x_aug + c)
 44 |         // So subtracting (H_x * x_aug + c) from the constraints, we get:
 45 |         // lb_constraint - H_x * x_aug - c <= (H_u + H_x * B_aug)u <= ub_constraint - H_x * x_aug - c
 46 | 
 47 |         Matrix<> B_aug = Matrix<>::Zero(N * (L - 1), M * (L - 1));
 48 |         Matrix<> x_aug = Vector<>::Zero(N * (L - 1));
 49 | 
 50 |         Matrix<> Q_aug = Matrix<>::Zero(N * (L - 1), N * (L - 1));
 51 |         Matrix<> R_aug = Matrix<>::Zero(M * (L - 1), M * (L - 1));
 52 |         Matrix<> q_aug = Matrix<>::Zero(1, N * (L - 1));
 53 |         Matrix<> r_aug = Matrix<>::Zero(1, M * (L - 1));
 54 | 
 55 |         Matrix<> lb_input_aug = Vector<>::Zero(M * (L - 1));
 56 |         Matrix<> ub_input_aug = Vector<>::Zero(M * (L - 1));
 57 |         Matrix<> lb_constraint_aug = Vector<>::Zero(C * (L - 1));
 58 |         Matrix<> ub_constraint_aug = Vector<>::Zero(C * (L - 1));
 59 | 
 60 |         Matrix<N> A_i = Matrix<N>::Identity();
 61 | 
 62 |         Matrix<> B_aug_row = Matrix<>::Zero(N, M * (L - 1));
 63 | 
 64 |         Matrix<> H_x = Matrix<>::Zero(C * (L - 1), N * (L - 1));
 65 |         Matrix<> H_u = Matrix<>::Zero(C * (L - 1), M * (L - 1));
 66 |         Matrix<> h_const = Matrix<>::Zero(C * (L - 1), 1);
 67 | 
 68 |         Vector<N> unforced_x = Vector<N>::Zero();
 69 | 
 70 |         Vector<N> x0 = Vector<N>::Zero();
 71 |         Vector<M> u0 = Vector<M>::Zero();
 72 |         for (int i = 0; i < (L - 1); i++) {
 73 |             auto linearized = linearize<N, M, System>(system, x0, u0, i);
 74 |             B_aug_row = linearized.A * B_aug_row;
 75 |             B_aug_row.template block<N, M>(0, M * i) = linearized.B;
 76 |             unforced_x = linearized.A * unforced_x + linearized.c;
 77 |             x_aug.template block<N, 1>(N * i, 0) = unforced_x;
 78 | 
 79 |             auto quadratized = quadratize<N, M, Cost>(cost, x0, u0, i);
 80 | 
 81 |             if (i != L - 2) {
 82 |                 Q_aug.template block<N, N>(N * i, N * i) = quadratized.Q;
 83 |                 q_aug.template block<1, N>(0, N * i) = quadratized.q;
 84 |             }
 85 | 
 86 |             R_aug.template block<M, M>(M * i, M * i) = quadratized.R;
 87 |             r_aug.template block<1, M>(0, M * i) = quadratized.r;
 88 | 
 89 |             auto constraint_linear = linearize<N, M, Constraint, C>(constraint, x0, u0, i);
 90 |             H_x.template block<C, N>(C * i, N * i) = constraint_linear.A;
 91 |             H_u.template block<C, M>(C * i, M * i) = constraint_linear.B;
 92 |             h_const.block<C, 1>(C * i, 0) = constraint_linear.c;
 93 | 
 94 |             lb_constraint_aug.template block<C, 1>(C * i, 0) = lb_constraint;
 95 |             ub_constraint_aug.template block<C, 1>(C * i, 0) = ub_constraint;
 96 | 
 97 |             lb_input_aug.template block<M, 1>(M * i, 0) = lb_input;
 98 |             ub_input_aug.template block<M, 1>(M * i, 0) = ub_input;
 99 |         }
100 | 
101 |         auto qf = quadratize_final<N, CostFinal>(cost_final, x0);
102 |         Q_aug.template block<N, N>(N * (L - 2), N * (L - 2)) = qf.Q;
103 |         q_aug.template block<1, N>(0, N * (L - 2)) = qf.q;
104 | 
105 |         Matrix<> cost_quadratic = R_aug + B_aug.transpose() * Q_aug * B_aug;
106 |         Matrix<> cost_linear = r_aug + q_aug * B_aug + x_aug.transpose() * Q_aug * B_aug;
107 | 
108 |         Matrix<> H_aug = H_u + H_x * B_aug;
109 |         lb_constraint_aug -= h_const - H_x * x_aug;
110 |         ub_constraint_aug -= h_const - H_x * x_aug;
111 | 
112 |         qpOASES::Options options;
113 |         options.printLevel = qpOASES::PL_NONE;
114 |         problem.setOptions(options);
115 | 
116 |         int nWSR = 1000000;
117 |         problem.init(cost_quadratic.data(), cost_linear.data(), H_aug.data(), lb_input_aug.data(), ub_input_aug.data(), lb_constraint_aug.data(), ub_constraint_aug.data(), nWSR, 0);
118 |     }
119 | 
120 |     void iterate(Vector<N> x0) {
121 |         Vector<> u0 = Vector<>::Zero(M * (L - 1));
122 |         problem.getPrimalSolution(u0.data());
123 | 
124 |         // Do a forward pass similar to the above
125 |         Matrix<> B_aug = Matrix<>::Zero(N * (L - 1), M * (L - 1));
126 |         Matrix<> x_aug = Vector<>::Zero(N * (L - 1));
127 | 
128 |         Matrix<> Q_aug = Matrix<>::Zero(N * (L - 1), N * (L - 1));
129 |         Matrix<> R_aug = Matrix<>::Zero(M * (L - 1), M * (L - 1));
130 |         Matrix<> q_aug = Matrix<>::Zero(1, N * (L - 1));
131 |         Matrix<> r_aug = Matrix<>::Zero(1, M * (L - 1));
132 | 
133 |         Matrix<> lb_input_aug = Vector<>::Zero(M * (L - 1));
134 |         Matrix<> ub_input_aug = Vector<>::Zero(M * (L - 1));
135 |         Matrix<> lb_constraint_aug = Vector<>::Zero(C * (L - 1));
136 |         Matrix<> ub_constraint_aug = Vector<>::Zero(C * (L - 1));
137 | 
138 |         Matrix<N> A_i = Matrix<N>::Identity();
139 | 
140 |         Matrix<> B_aug_row = Matrix<>::Zero(N, M * (L - 1));
141 | 
142 |         Matrix<> H_x = Matrix<>::Zero(C * (L - 1), N * (L - 1));
143 |         Matrix<> H_u = Matrix<>::Zero(C * (L - 1), M * (L - 1));
144 |         Matrix<> h_const = Matrix<>::Zero(C * (L - 1), 1);
145 | 
146 |         Vector<N> unforced_x = x0;
147 | 
148 |         Vector<N> x_hat = x0;
149 |         for (int i = 0; i < (L - 1); i++) {
150 |             Vector<M> u_hat = u0.template block<M, 1>(M * i, 0);
151 | 
152 |             auto linearized = linearize<N, M, System>(system, x_hat, u_hat, i);
153 |             B_aug_row = linearized.A * B_aug_row;
154 |             B_aug_row.template block<N, M>(0, M * i) = linearized.B;
155 |             B_aug.template block(N * i, 0, N, M * (L - 1)) = B_aug_row;
156 |             unforced_x = linearized.A * unforced_x + linearized.c;
157 |             x_aug.template block<N, 1>(N * i, 0) = unforced_x;
158 | 
159 |             x_hat = system(x_hat, u_hat, i);
160 | 
161 |             auto quadratized = quadratize<N, M, Cost>(cost, x_hat, u_hat, i);
162 | 
163 |             if (i != L - 2) {
164 |                 Q_aug.template block<N, N>(N * i, N * i) = quadratized.Q;
165 |                 q_aug.template block<1, N>(0, N * i) = quadratized.q;
166 |             }
167 | 
168 |             R_aug.template block<M, M>(M * i, M * i) = quadratized.R;
169 |             r_aug.template block<1, M>(0, M * i) = quadratized.r;
170 | 
171 |             auto constraint_linear = linearize<N, M, Constraint, C>(constraint, x_hat, u_hat, i);
172 |             H_x.template block<C, N>(C * i, N * i) = constraint_linear.A;
173 |             H_u.template block<C, M>(C * i, M * i) = constraint_linear.B;
174 |             h_const.block<C, 1>(C * i, 0) = constraint_linear.c;
175 | 
176 |             lb_constraint_aug.template block<C, 1>(C * i, 0) = lb_constraint;
177 |             ub_constraint_aug.template block<C, 1>(C * i, 0) = ub_constraint;
178 | 
179 |             lb_input_aug.template block<M, 1>(M * i, 0) = lb_input;
180 |             ub_input_aug.template block<M, 1>(M * i, 0) = ub_input;
181 |         }
182 | 
183 |         auto qf = quadratize_final<N, CostFinal>(cost_final, x0);
184 |         Q_aug.template block<N, N>(N * (L - 2), N * (L - 2)) = qf.Q;
185 |         q_aug.template block<1, N>(0, N * (L - 2)) = qf.q;
186 | 
187 |         Matrix<> cost_quadratic = R_aug + B_aug.transpose() * Q_aug * B_aug;
188 |         Matrix<> cost_linear = r_aug + q_aug * B_aug + x_aug.transpose() * Q_aug * B_aug;
189 | 
190 |         Matrix<> H_aug = H_u + H_x * B_aug;
191 |         lb_constraint_aug -= h_const - H_x * x_aug;
192 |         ub_constraint_aug -= h_const - H_x * x_aug;
193 | 
194 |         int nWSR = 1000000;
195 |         problem.hotstart(cost_quadratic.data(), cost_linear.data(), H_aug.data(), lb_input_aug.data(), ub_input_aug.data(), lb_constraint_aug.data(), ub_constraint_aug.data(), nWSR, 0);
196 |     }
197 | 
198 |     Vector<> plan(Vector<N> x0, Vector<N> r) {
199 |         for (int i = 0; i < 50; i++) {
200 |              iterate(x0);
201 |         }
202 |         Vector<> u0 = Vector<>::Zero(M * (L - 1));
203 |         problem.getPrimalSolution(u0.data());
204 |         return u0;
205 |     }
206 | 
207 |     Vector<M> control(Vector<N> x0, Vector<N> r) {
208 |         return plan(x0, r).template block<M, 1>(0, 0);
209 |     }
210 | 
211 | protected:
212 |     qpOASES::SQProblem problem;
213 | 
214 |     System system;
215 |     Constraint constraint;
216 |     Cost cost;
217 |     CostFinal cost_final;
218 |     Vector<C> lb_constraint, ub_constraint;
219 |     Vector<M> lb_input, ub_input;
220 | 
221 |     int L;
222 | };
223 | 


--------------------------------------------------------------------------------
/mpc/nmpc_demo.cpp:
--------------------------------------------------------------------------------
 1 | #include "nmpc.h"
 2 | #include <iostream>
 3 | 
 4 | constexpr double dt = 0.1;
 5 | 
 6 | struct System {
 7 |     Vector<2> operator()(Vector<2> x, Vector<1> u, int t) {
 8 |         return Vector<2>(x(1), u(0) - 9.81 * std::sin(x(0)));
 9 |     }
10 | };
11 | 
12 | struct Cost {
13 |     double operator()(Vector<2> x, Vector<1> u, int t) {
14 |         return 0; // dt * (u(0) * u(0) + 0.2 * (x(0) - 3.14) * (x(0) - 3.14));
15 |     }
16 | };
17 | 
18 | struct CostFinal {
19 |     double operator()(Vector<2> x) {
20 |         return 400 * (x(0) - 3.14) * (x(0) - 3.14) + 100 * x(1) * x(1);
21 |     }
22 | };
23 | 
24 | struct Constraint {
25 |     Vector<1> operator()(Vector<2> x, Vector<1> u, int t) {
26 |         return Vector<1>(0);
27 |     }
28 | };
29 | 
30 | int main() {
31 |     System system;
32 |     Cost cost;
33 |     CostFinal cost_final;
34 |     Constraint constraint;
35 |     Discretized<2, 1, System> disc{dt, system};
36 |     int N = 60;
37 |     NonlinearModelPredictiveController<2, 1, 1, Discretized<2, 1, System>, Cost, CostFinal, Constraint> nmpc(
38 |             disc, cost, cost_final, constraint,
39 |             Vector<1>(-7), Vector<1>(7),
40 |             Vector<1>(-2), Vector<1>(2),
41 |             N + 1
42 |         );
43 | 
44 |     Vector<2> x(0, 0);
45 |     Vector<> u = nmpc.plan(x, Vector<2>::Zero());
46 |     for (int i = 0; i < N; i++) {
47 |         std::cout << x(0) << ", " << x(1) << ", " << u(i) << std::endl;
48 |         x = disc(x, Vector<1>(u(i)), i);
49 |     }
50 |     std::cout << x(0) << ", " << x(1) << std::endl;
51 | }
52 | 


--------------------------------------------------------------------------------
/third_party/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_subdirectory (qpOASES)
2 | add_subdirectory (googletest)
3 | 


--------------------------------------------------------------------------------