autogenu-jupyter
An automatic code generator and the continuation/GMRES (C/GMRES) based numerical solvers for nonlinear MPC
Loading...
Searching...
No Matches
multiple_shooting_nlp.hpp
Go to the documentation of this file.
1#ifndef CGMRES__MULTIPLE_SHOOTING_NLP_HPP_
2#define CGMRES__MULTIPLE_SHOOTING_NLP_HPP_
3
4#include <array>
5
6#include "cgmres/types.hpp"
7#include "cgmres/horizon.hpp"
8
11
12namespace cgmres {
13namespace detail {
14
15template <class OCP, int N>
17public:
18 static constexpr int nx = OCP::nx;
19 static constexpr int nu = OCP::nu;
20 static constexpr int nc = OCP::nc;
21 static constexpr int nuc = nu + nc;
22 static constexpr int nub = OCP::nub;
23 static constexpr int dim = nuc * N;
24
26 : ocp_(ocp),
27 horizon_(horizon) {
28 static_assert(OCP::nx > 0);
29 static_assert(OCP::nu > 0);
30 static_assert(OCP::nc >= 0);
31 static_assert(OCP::nub >= 0);
32 static_assert(N > 0);
33 }
34
36
38
39 template <typename VectorType>
40 void eval_fonc_hu(const Scalar t, const MatrixBase<VectorType>& x0, const Vector<dim>& solution,
41 const std::array<Vector<nx>, N+1>& x, const std::array<Vector<nx>, N+1>& lmd,
42 Vector<dim>& fonc_hu) {
43 assert(x0.size());
44 const Scalar T = horizon_.T(t);
45 const Scalar dt = T / N;
46 assert(T >= 0);
47 // Compute the erros in the first order necessary conditions (FONC)
48 ocp_.eval_hu(t, x0.derived().data(), solution.template head<nuc>().data(), lmd[1].data(),
49 fonc_hu.template head<nuc>().data());
50 for (size_t i=1; i<N; ++i) {
51 ocp_.eval_hu(t+i*dt, x[i].data(), solution.template segment<nuc>(nuc*i).data(),
52 lmd[i+1].data(), fonc_hu.template segment<nuc>(nuc*i).data());
53 }
54 }
55
56 template <typename VectorType>
57 void eval_fonc_f(const Scalar t, const MatrixBase<VectorType>& x0, const Vector<dim>& solution,
58 const std::array<Vector<nx>, N+1>& x,
59 std::array<Vector<nx>, N+1>& fonc_f) {
60 const Scalar T = horizon_.T(t);
61 const Scalar dt = T / N;
62 assert(T >= 0);
63 // Compute optimality error for state.
64 ocp_.eval_f(t, x0.derived().data(), solution.template head<nuc>().data(), dx_.data());
65 fonc_f[0] = x[1] - x0 - dt * dx_;
66 for (size_t i=1; i<N; ++i) {
67 ocp_.eval_f(t+i*dt, x[i].data(), solution.template segment<nuc>(nuc*i).data(), dx_.data());
68 fonc_f[i] = x[i+1] - x[i] - dt * dx_;
69 }
70 }
71
72 template <typename VectorType>
73 void retrive_x(const Scalar t, const MatrixBase<VectorType>& x0, const Vector<dim>& solution,
74 std::array<Vector<nx>, N+1>& x,
75 const std::array<Vector<nx>, N+1>& fonc_f) {
76 const Scalar T = horizon_.T(t);
77 const Scalar dt = T / N;
78 assert(T >= 0);
79 // Compute optimality error for state.
80 ocp_.eval_f(t, x0.derived().data(), solution.template head<nuc>().data(), dx_.data());
81 x[1] = x0 + dt * dx_ + fonc_f[0];
82 for (size_t i=1; i<N; ++i) {
83 ocp_.eval_f(t+i*dt, x[i].data(), solution.template segment<nuc>(nuc*i).data(), dx_.data());
84 x[i+1] = x[i] + dt * dx_ + fonc_f[i];
85 }
86 }
87
88 template <typename VectorType>
89 void eval_fonc_hx(const Scalar t, const MatrixBase<VectorType>& x0, const Vector<dim>& solution,
90 const std::array<Vector<nx>, N+1>& x, const std::array<Vector<nx>, N+1>& lmd,
91 std::array<Vector<nx>, N+1>& fonc_hx) {
92 const Scalar T = horizon_.T(t);
93 const Scalar dt = T / N;
94 assert(T >= 0);
95 // Compute optimality error for lambda.
96 ocp_.eval_phix(t+T, x[N].data(), dx_.data());
97 fonc_hx[N] = lmd[N] - dx_;
98 for (size_t i=N-1; i>=1; --i) {
99 ocp_.eval_hx(t+i*dt, x[i].data(), solution.template segment<nuc>(nuc*i).data(),
100 lmd[i+1].data(), dx_.data());
101 fonc_hx[i] = lmd[i] - lmd[i+1] - dt * dx_;
102 }
103 }
104
105 template <typename VectorType>
106 void retrive_lmd(const Scalar t, const MatrixBase<VectorType>& x0, const Vector<dim>& solution,
107 const std::array<Vector<nx>, N+1>& x, std::array<Vector<nx>, N+1>& lmd,
108 const std::array<Vector<nx>, N+1>& fonc_hx) {
109 const Scalar T = horizon_.T(t);
110 const Scalar dt = T / N;
111 assert(T >= 0);
112 // Compute optimality error for state.
113 ocp_.eval_phix(t+T, x[N].data(), dx_.data());
114 lmd[N] = dx_ + fonc_hx[N];
115 for (size_t i=N-1; i>=1; --i) {
116 ocp_.eval_hx(t+i*dt, x[i].data(), solution.template segment<nuc>(nuc*i).data(),
117 lmd[i+1].data(), dx_.data());
118 lmd[i] = lmd[i+1] + dt * dx_ + fonc_hx[i];
119 }
120 }
121
122 void eval_fonc_hu(const Vector<dim>& solution,
123 const std::array<Vector<nub>, N>& dummy,
124 const std::array<Vector<nub>, N>& mu,
125 Vector<dim>& fonc_hu) const {
126 ubounds::eval_fonc_hu<OCP, N>(ocp_, solution, dummy, mu, fonc_hu);
127 }
128
129 void eval_fonc_hdummy(const Vector<dim>& solution,
130 const std::array<Vector<nub>, N>& dummy,
131 const std::array<Vector<nub>, N>& mu,
132 std::array<Vector<nub>, N>& fonc_hdummy) const {
133 ubounds::eval_fonc_hdummy<OCP, N>(ocp_, solution, dummy, mu, fonc_hdummy);
134 }
135
136 void eval_fonc_hmu(const Vector<dim>& solution,
137 const std::array<Vector<nub>, N>& dummy,
138 const std::array<Vector<nub>, N>& mu,
139 std::array<Vector<nub>, N>& fonc_hmu) const {
140 ubounds::eval_fonc_hmu<OCP, N>(ocp_, solution, dummy, mu, fonc_hmu);
141 }
142
143 static void multiply_hdummy_inv(const std::array<Vector<nub>, N>& dummy,
144 const std::array<Vector<nub>, N>& mu,
145 const std::array<Vector<nub>, N>& fonc_hdummy,
146 const std::array<Vector<nub>, N>& fonc_hmu,
147 std::array<Vector<nub>, N>& fonc_hdummy_inv) {
148 ubounds::multiply_hdummy_inv<OCP, N>(dummy, mu, fonc_hdummy, fonc_hmu,
149 fonc_hdummy_inv);
150 }
151
152 static void multiply_hmu_inv(const std::array<Vector<nub>, N>& dummy,
153 const std::array<Vector<nub>, N>& mu,
154 const std::array<Vector<nub>, N>& fonc_hdummy,
155 const std::array<Vector<nub>, N>& fonc_hmu,
156 const std::array<Vector<nub>, N>& fonc_hdummy_inv,
157 std::array<Vector<nub>, N>& fonc_hmu_inv) {
158 ubounds::multiply_hmu_inv<OCP, N>(dummy, mu, fonc_hdummy, fonc_hmu,
159 fonc_hdummy_inv, fonc_hmu_inv);
160 }
161
163 const std::array<Vector<OCP::nub>, N>& dummy,
164 const std::array<Vector<OCP::nub>, N>& mu,
165 const Vector<OCP::nuc*N>& solution_update,
166 std::array<Vector<OCP::nub>, N>& dummy_update) {
167 ubounds::retrive_dummy_update<OCP, N>(ocp_, solution, dummy, mu, solution_update, dummy_update);
168 }
169
171 const std::array<Vector<OCP::nub>, N>& dummy,
172 const std::array<Vector<OCP::nub>, N>& mu,
173 const Vector<OCP::nuc*N>& solution_update,
174 std::array<Vector<OCP::nub>, N>& mu_update) {
175 ubounds::retrive_mu_update<OCP, N>(ocp_, solution, dummy, mu, solution_update, mu_update);
176 }
177
178 void clip_dummy(std::array<Vector<OCP::nub>, N>& dummy, const Scalar min) {
179 ubounds::clip_dummy<OCP, N>(dummy, min);
180 }
181
182 void synchronize_ocp() { ocp_.synchronize(); }
183
184 const OCP& ocp() const { return ocp_; }
185
186 const Horizon& horizon() const { return horizon_; }
187
188 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
189
190private:
191 OCP ocp_;
192 Horizon horizon_;
193 Vector<nx> dx_;
194};
195
196} // namespace detail
197} // namespace cgmres
198
199#endif // CGMRES__MULTIPLE_SHOOTING_NLP_HPP_
Horizon of MPC.
Definition: horizon.hpp:18
Scalar T(const Scalar t) const
Gets the length of the horizon.
Definition: horizon.hpp:50
Definition: multiple_shooting_nlp.hpp:16
MultipleShootingNLP(const OCP &ocp, const Horizon &horizon)
Definition: multiple_shooting_nlp.hpp:25
void eval_fonc_f(const Scalar t, const MatrixBase< VectorType > &x0, const Vector< dim > &solution, const std::array< Vector< nx >, N+1 > &x, std::array< Vector< nx >, N+1 > &fonc_f)
Definition: multiple_shooting_nlp.hpp:57
static constexpr int nc
Definition: multiple_shooting_nlp.hpp:20
void clip_dummy(std::array< Vector< OCP::nub >, N > &dummy, const Scalar min)
Definition: multiple_shooting_nlp.hpp:178
void eval_fonc_hdummy(const Vector< dim > &solution, const std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu, std::array< Vector< nub >, N > &fonc_hdummy) const
Definition: multiple_shooting_nlp.hpp:129
static constexpr int dim
Definition: multiple_shooting_nlp.hpp:23
void eval_fonc_hu(const Scalar t, const MatrixBase< VectorType > &x0, const Vector< dim > &solution, const std::array< Vector< nx >, N+1 > &x, const std::array< Vector< nx >, N+1 > &lmd, Vector< dim > &fonc_hu)
Definition: multiple_shooting_nlp.hpp:40
static void multiply_hdummy_inv(const std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu, const std::array< Vector< nub >, N > &fonc_hdummy, const std::array< Vector< nub >, N > &fonc_hmu, std::array< Vector< nub >, N > &fonc_hdummy_inv)
Definition: multiple_shooting_nlp.hpp:143
static constexpr int nub
Definition: multiple_shooting_nlp.hpp:22
void retrive_dummy_update(const Vector< OCP::nuc *N > &solution, const std::array< Vector< OCP::nub >, N > &dummy, const std::array< Vector< OCP::nub >, N > &mu, const Vector< OCP::nuc *N > &solution_update, std::array< Vector< OCP::nub >, N > &dummy_update)
Definition: multiple_shooting_nlp.hpp:162
void retrive_x(const Scalar t, const MatrixBase< VectorType > &x0, const Vector< dim > &solution, std::array< Vector< nx >, N+1 > &x, const std::array< Vector< nx >, N+1 > &fonc_f)
Definition: multiple_shooting_nlp.hpp:73
static constexpr int nuc
Definition: multiple_shooting_nlp.hpp:21
void eval_fonc_hx(const Scalar t, const MatrixBase< VectorType > &x0, const Vector< dim > &solution, const std::array< Vector< nx >, N+1 > &x, const std::array< Vector< nx >, N+1 > &lmd, std::array< Vector< nx >, N+1 > &fonc_hx)
Definition: multiple_shooting_nlp.hpp:89
void eval_fonc_hmu(const Vector< dim > &solution, const std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu, std::array< Vector< nub >, N > &fonc_hmu) const
Definition: multiple_shooting_nlp.hpp:136
static constexpr int nx
Definition: multiple_shooting_nlp.hpp:18
static constexpr int nu
Definition: multiple_shooting_nlp.hpp:19
void synchronize_ocp()
Definition: multiple_shooting_nlp.hpp:182
const OCP & ocp() const
Definition: multiple_shooting_nlp.hpp:184
void retrive_lmd(const Scalar t, const MatrixBase< VectorType > &x0, const Vector< dim > &solution, const std::array< Vector< nx >, N+1 > &x, std::array< Vector< nx >, N+1 > &lmd, const std::array< Vector< nx >, N+1 > &fonc_hx)
Definition: multiple_shooting_nlp.hpp:106
void retrive_mu_update(const Vector< OCP::nuc *N > &solution, const std::array< Vector< OCP::nub >, N > &dummy, const std::array< Vector< OCP::nub >, N > &mu, const Vector< OCP::nuc *N > &solution_update, std::array< Vector< OCP::nub >, N > &mu_update)
Definition: multiple_shooting_nlp.hpp:170
static void multiply_hmu_inv(const std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu, const std::array< Vector< nub >, N > &fonc_hdummy, const std::array< Vector< nub >, N > &fonc_hmu, const std::array< Vector< nub >, N > &fonc_hdummy_inv, std::array< Vector< nub >, N > &fonc_hmu_inv)
Definition: multiple_shooting_nlp.hpp:152
const Horizon & horizon() const
Definition: multiple_shooting_nlp.hpp:186
void eval_fonc_hu(const Vector< dim > &solution, const std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu, Vector< dim > &fonc_hu) const
Definition: multiple_shooting_nlp.hpp:122
Definition: continuation_gmres.hpp:11
Eigen::Matrix< Scalar, size, 1 > Vector
Alias of Eigen::Vector.
Definition: types.hpp:23
double Scalar
Alias of double.
Definition: types.hpp:11
Eigen::MatrixBase< MatrixType > MatrixBase
Alias of Eigen::MatrixBase.
Definition: types.hpp:29