autogenu-jupyter
An automatic code generator and the continuation/GMRES (C/GMRES) based numerical solvers for nonlinear MPC
Loading...
Searching...
No Matches
multiple_shooting_cgmres_solver.hpp
Go to the documentation of this file.
1#ifndef CGMRES__MULTIPLE_SHOOTING_CGMRES_SOLVER_HPP_
2#define CGMRES__MULTIPLE_SHOOTING_CGMRES_SOLVER_HPP_
3
4#include <array>
5#include <stdexcept>
6#include <iostream>
7
8#include "cgmres/types.hpp"
10#include "cgmres/timer.hpp"
11
15
16namespace cgmres {
17
25template <class OCP, int N, int kmax>
27public:
31 static constexpr int nx = OCP::nx;
32
36 static constexpr int nu = OCP::nu;
37
41 static constexpr int nc = OCP::nc;
42
46 static constexpr int nuc = nu + nc;
47
51 static constexpr int nub = OCP::nub;
52
56 static constexpr int dim = nuc * N;
57
61
68 MultipleShootingCGMRESSolver(const OCP& ocp, const Horizon& horizon,
69 const SolverSettings& settings)
70 : continuation_gmres_(MultipleShootingNLP_(ocp, horizon), settings.finite_difference_epsilon, settings.zeta),
71 gmres_(),
72 settings_(settings),
73 solution_(Vector<dim>::Zero()),
74 solution_update_(Vector<dim>::Zero()) {
75 std::fill(uopt_.begin(), uopt_.end(), Vector<nu>::Zero());
76 std::fill(ucopt_.begin(), ucopt_.end(), Vector<nuc>::Zero());
77 std::fill(xopt_.begin(), xopt_.end(), Vector<nx>::Zero());
78 std::fill(lmdopt_.begin(), lmdopt_.end(), Vector<nx>::Zero());
79 if constexpr (nub > 0) {
80 std::fill(dummyopt_.begin(), dummyopt_.end(), Vector<nub>::Zero());
81 std::fill(muopt_.begin(), muopt_.end(), Vector<nub>::Zero());
82 }
83
84 if (settings.finite_difference_epsilon <= 0.0) {
85 throw std::invalid_argument("[MultipleShootingCGMRESSolver]: 'settings.finite_difference_epsilon' must be positive!");
86 }
87 if (settings.sampling_time <= 0.0) {
88 throw std::invalid_argument("[MultipleShootingCGMRESSolver]: 'settings.sampling_time' must be positive!");
89 }
90 if (settings.zeta <= 0.0) {
91 throw std::invalid_argument("[ContinuationGMRESCondensing]: 'settings.zeta' must be positive!");
92 }
93 }
94
99
104
109 template <typename VectorType>
111 if (u.size() != nu) {
112 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_u] u.size() must be " + std::to_string(nu));
113 }
114 for (size_t i=0; i<N; ++i) {
115 uopt_[i] = u;
116 }
117 for (size_t i=0; i<N; ++i) {
118 ucopt_[i].template head<nu>() = u;
119 }
120 setInnerSolution();
121 }
122
128 template <typename VectorType>
130 if (uc.size() != nuc) {
131 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_uc] uc.size() must be " + std::to_string(nuc));
132 }
133 for (size_t i=0; i<N; ++i) {
134 uopt_[i] = uc.template head<nu>();
135 }
136 for (size_t i=0; i<N; ++i) {
137 ucopt_[i] = uc;
138 }
139 setInnerSolution();
140 }
141
146 template <typename VectorType>
148 if (x.size() != nx) {
149 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_x] x.size() must be " + std::to_string(nx));
150 }
151 for (size_t i=1; i<=N; ++i) {
152 xopt_[i] = x;
153 }
154 }
155
160 template <typename VectorType>
162 if (lmd.size() != nx) {
163 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_lmd] lmd.size() must be " + std::to_string(nx));
164 }
165 for (size_t i=1; i<=N; ++i) {
166 lmdopt_[i] = lmd;
167 }
168 }
169
174 template <typename VectorType>
176 if (dummy.size() != nub) {
177 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_dummy] dummy.size() must be " + std::to_string(nub));
178 }
179 for (size_t i=0; i<N; ++i) {
180 dummyopt_[i] = dummy;
181 }
182 }
183
188 template <typename VectorType>
190 if (mu.size() != nub) {
191 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_mu] mu.size() must be " + std::to_string(nub));
192 }
193 for (size_t i=0; i<N; ++i) {
194 muopt_[i] = mu;
195 }
196 }
197
202 template <typename T>
203 void set_u_array(const T& u_array) {
204 if (u_array.size() != N) {
205 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_u_array]: 'u_array.size()' must be "+std::to_string(N));
206 }
207 for (size_t i=0; i<N; ++i) {
208 if (u_array[i].size() != nu) {
209 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_u_array] u_array[i].size() must be " + std::to_string(nu));
210 }
211 uopt_[i] = u_array[i];
212 ucopt_[i].template head<nu>() = u_array[i];
213 }
214 setInnerSolution();
215 }
216
222 template <typename T>
223 void set_uc_array(const T& uc_array) {
224 if (uc_array.size() != N) {
225 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_uc_array]: 'uc_array.size()' must be "+std::to_string(N));
226 }
227 for (size_t i=0; i<N; ++i) {
228 if (uc_array[i].size() != nuc) {
229 throw std::invalid_argument("[MultipleShootingCGMRESSolver::uc_array] uc_array[i].size() must be " + std::to_string(nuc));
230 }
231 uopt_[i] = uc_array[i].template head<nu>();
232 ucopt_[i] = uc_array[i];
233 }
234 setInnerSolution();
235 }
236
241 template <typename T>
242 void set_x_array(const T& x_array) {
243 if (x_array.size() != N+1) {
244 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_x_array]: 'x_array.size()' must be "+std::to_string(N+1));
245 }
246 for (size_t i=1; i<=N; ++i) {
247 if (x_array.size() != nx) {
248 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_x_array] x_array[i].size() must be " + std::to_string(nx));
249 }
250 xopt_[i] = x_array[i];
251 }
252 }
253
258 template <typename T>
259 void set_lmd_array(const T& lmd_array) {
260 if (lmd_array.size() != N+1) {
261 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_lmd_array]: 'lmd_array.size()' must be "+std::to_string(N+1));
262 }
263 for (size_t i=1; i<=N; ++i) {
264 if (lmd_array.size() != nx) {
265 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_lmd_array] lmd_array[i].size() must be " + std::to_string(nx));
266 }
267 lmdopt_[i] = lmd_array[i];
268 }
269 }
270
276 template <typename T>
277 void set_dummy_array(const T& dummy_array) {
278 if (dummy_array.size() != N) {
279 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_dummy_array]: 'dummy_array.size()' must be "+std::to_string(N));
280 }
281 for (size_t i=0; i<N; ++i) {
282 if (dummy_array[i].size() != nub) {
283 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_dummy_array] dummy_array[i].size() must be " + std::to_string(nub));
284 }
285 dummyopt_[i] = dummy_array[i];
286 }
287 }
288
294 template <typename T>
295 void set_mu_array(const T& mu_array) {
296 if (mu_array.size() != N) {
297 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_mu_array]: 'mu_array.size()' must be "+std::to_string(N));
298 }
299 for (size_t i=0; i<N; ++i) {
300 if (mu_array[i].size() != nub) {
301 throw std::invalid_argument("[MultipleShootingCGMRESSolver::set_mu_array] mu_array[i].size() must be " + std::to_string(nub));
302 }
303 muopt_[i] = mu_array[i];
304 }
305 }
306
312 template <typename VectorType>
313 void init_x(const Scalar t, const MatrixBase<VectorType>& x) {
314 if (x.size() != nx) {
315 throw std::invalid_argument("[MultipleShootingCGMRESSolver::init_x] x.size() must be " + std::to_string(nx));
316 }
317 continuation_gmres_.retrive_x(t, x, solution_, xopt_);
318 }
319
325 template <typename VectorType>
326 void init_lmd(const Scalar t, const MatrixBase<VectorType>& x) {
327 if (x.size() != nx) {
328 throw std::invalid_argument("[MultipleShootingCGMRESSolver::init_lmd] x.size() must be " + std::to_string(nx));
329 }
330 continuation_gmres_.retrive_lmd(t, x, solution_, xopt_, lmdopt_);
331 }
332
338 template <typename VectorType>
339 void init_x_lmd(const Scalar t, const MatrixBase<VectorType>& x) {
340 if (x.size() != nx) {
341 throw std::invalid_argument("[MultipleShootingCGMRESSolver::init_x_lmd] x.size() must be " + std::to_string(nx));
342 }
343 continuation_gmres_.retrive_x(t, x, solution_, xopt_);
344 continuation_gmres_.retrive_lmd(t, x, solution_, xopt_, lmdopt_);
345 }
346
351 continuation_gmres_.retrive_dummy(solution_, dummyopt_, muopt_, settings_.min_dummy);
352 continuation_gmres_.retrive_mu(solution_, dummyopt_, muopt_);
353 }
354
359 const std::array<Vector<nu>, N>& uopt() const { return uopt_; }
360
365 const std::array<Vector<nuc>, N>& ucopt() const { return ucopt_; }
366
371 const std::array<Vector<nx>, N+1>& xopt() const { return xopt_; }
372
377 const std::array<Vector<nx>, N+1>& lmdopt() const { return lmdopt_; }
378
383 const std::array<Vector<nub>, N>& dummyopt() const { return dummyopt_; }
384
389 const std::array<Vector<nub>, N>& muopt() const { return muopt_; }
390
395 Scalar optError() const { return continuation_gmres_.optError(); }
396
403 template <typename VectorType>
405 if (x.size() != nx) {
406 throw std::invalid_argument("[MultipleShootingCGMRESSolver::optError] x.size() must be " + std::to_string(nx));
407 }
408 continuation_gmres_.synchronize_ocp();
409 continuation_gmres_.eval_fonc(t, x, solution_, xopt_, lmdopt_, dummyopt_, muopt_);
410 return optError();
411 }
412
418 template <typename VectorType>
419 void update(const Scalar t, const MatrixBase<VectorType>& x) {
420 if (x.size() != nx) {
421 throw std::invalid_argument("[MultipleShootingCGMRESSolver::update] x.size() must be " + std::to_string(nx));
422 }
423 if (settings_.verbose_level >= 1) {
424 std::cout << "\n======================= update solution with C/GMRES =======================" << std::endl;
425 }
426
427 if (settings_.profile_solver) timer_.tick();
428 continuation_gmres_.synchronize_ocp();
429 const auto gmres_iter
430 = gmres_.template solve<const Scalar, const MatrixBase<VectorType>&, const Vector<dim>&,
431 const std::array<Vector<nx>, N+1>&, const std::array<Vector<nx>, N+1>&,
432 const std::array<Vector<nub>, N>&, const std::array<Vector<nub>, N>&>(
433 continuation_gmres_, t, x.derived(), solution_, xopt_, lmdopt_, dummyopt_, muopt_, solution_update_);
434 const auto opt_error = continuation_gmres_.optError();
435 continuation_gmres_.expansion(t, x, solution_, xopt_, lmdopt_, dummyopt_, muopt_,
436 solution_update_, settings_.sampling_time, settings_.min_dummy);
437 solution_.noalias() += settings_.sampling_time * solution_update_;
438 retriveSolution();
439 if (settings_.profile_solver) timer_.tock();
440
441 // verbose
442 if (settings_.verbose_level >= 1) {
443 std::cout << "opt error: " << opt_error << std::endl;
444 }
445 if (settings_.verbose_level >= 2) {
446 std::cout << "number of GMRES iter: " << gmres_iter << " (kmax: " << kmax << ")" << std::endl;
447 }
448 }
449
455 return timer_.getProfile();
456 }
457
458 void disp(std::ostream& os) const {
459 os << "Multiple shooting CGMRES solver: " << std::endl;
460 os << " N: " << N << std::endl;
461 os << " kmax: " << kmax << "\n" << std::endl;
462 os << continuation_gmres_.get_nlp().ocp() << std::endl;
463 os << continuation_gmres_.get_nlp().horizon() << std::endl;
464 os << settings_ << std::endl;
465 os << timer_.getProfile() << std::endl;
466 }
467
468 friend std::ostream& operator<<(std::ostream& os, const MultipleShootingCGMRESSolver& solver) {
469 solver.disp(os);
470 return os;
471 }
472
473 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
474
475private:
476 ContinuationGMRES_ continuation_gmres_;
477 MatrixFreeGMRES_ gmres_;
478 SolverSettings settings_;
479 Timer timer_;
480
481 std::array<Vector<nu>, N> uopt_;
482 std::array<Vector<nuc>, N> ucopt_;
483 std::array<Vector<nx>, N+1> xopt_;
484 std::array<Vector<nx>, N+1> lmdopt_;
485 std::array<Vector<nub>, N> dummyopt_;
486 std::array<Vector<nub>, N> muopt_;
487
488 Vector<dim> solution_, solution_update_;
489
490 void setInnerSolution() {
491 for (size_t i=0; i<N; ++i) {
492 solution_.template segment<nuc>(i*nuc) = ucopt_[i];
493 }
494 }
495
496 void retriveSolution() {
497 for (size_t i=0; i<N; ++i) {
498 uopt_[i] = solution_.template segment<nu>(i*nuc);
499 ucopt_[i] = solution_.template segment<nuc>(i*nuc);
500 }
501 }
502
503};
504
505} // namespace cgmres
506
507#endif // CGMRES__MULTIPLE_SHOOTING_CGMRES_SOLVER_HPP_
Horizon of MPC.
Definition: horizon.hpp:18
Multiple-shooting C/GMRES solver for nonlinear MPC.
Definition: multiple_shooting_cgmres_solver.hpp:26
friend std::ostream & operator<<(std::ostream &os, const MultipleShootingCGMRESSolver &solver)
Definition: multiple_shooting_cgmres_solver.hpp:468
void set_x(const MatrixBase< VectorType > &x)
Sets the state vector.
Definition: multiple_shooting_cgmres_solver.hpp:147
Scalar optError(const Scalar t, const MatrixBase< VectorType > &x)
Computes and gets the l2-norm of the current optimality errors.
Definition: multiple_shooting_cgmres_solver.hpp:404
const std::array< Vector< nuc >, N > & ucopt() const
Getter of the optimal solution.
Definition: multiple_shooting_cgmres_solver.hpp:365
void set_mu_array(const T &mu_array)
Sets the array of the Lagrange multiplier with respect to the control input bounds constraint.
Definition: multiple_shooting_cgmres_solver.hpp:295
const std::array< Vector< nx >, N+1 > & lmdopt() const
Getter of the optimal solution.
Definition: multiple_shooting_cgmres_solver.hpp:377
TimingProfile getProfile() const
Get timing result as TimingProfile.
Definition: multiple_shooting_cgmres_solver.hpp:454
void set_dummy_array(const T &dummy_array)
Sets the array of the dummy input vector with respect to the control input bounds constraint.
Definition: multiple_shooting_cgmres_solver.hpp:277
static constexpr int nu
Dimension of the control input.
Definition: multiple_shooting_cgmres_solver.hpp:36
Scalar optError() const
Gets the l2-norm of the current optimality errors.
Definition: multiple_shooting_cgmres_solver.hpp:395
detail::ContinuationGMRESCondensing< MultipleShootingNLP_ > ContinuationGMRES_
Definition: multiple_shooting_cgmres_solver.hpp:59
void set_lmd_array(const T &lmd_array)
Sets the array of costate vector.
Definition: multiple_shooting_cgmres_solver.hpp:259
void set_dummy(const MatrixBase< VectorType > &dummy)
Sets the dummy input vector with respect to the control input bounds constraint.
Definition: multiple_shooting_cgmres_solver.hpp:175
static constexpr int nub
Dimension of the bound constraints on the control input.
Definition: multiple_shooting_cgmres_solver.hpp:51
void init_lmd(const Scalar t, const MatrixBase< VectorType > &x)
Initializes the costate vectors by simulating the system costate dynamics over the horizon.
Definition: multiple_shooting_cgmres_solver.hpp:326
void disp(std::ostream &os) const
Definition: multiple_shooting_cgmres_solver.hpp:458
void set_x_array(const T &x_array)
Sets the array of state vector.
Definition: multiple_shooting_cgmres_solver.hpp:242
~MultipleShootingCGMRESSolver()=default
Default destructor.
void update(const Scalar t, const MatrixBase< VectorType > &x)
Updates the solution by performing C/GMRES method.
Definition: multiple_shooting_cgmres_solver.hpp:419
MultipleShootingCGMRESSolver()=default
Default constructor.
void init_x(const Scalar t, const MatrixBase< VectorType > &x)
Initializes the state vectors by simulating the system dynamics over the horizon.
Definition: multiple_shooting_cgmres_solver.hpp:313
static constexpr int nc
Dimension of the equality constraints.
Definition: multiple_shooting_cgmres_solver.hpp:41
void set_lmd(const MatrixBase< VectorType > &lmd)
Sets the costate vector.
Definition: multiple_shooting_cgmres_solver.hpp:161
void set_uc_array(const T &uc_array)
Sets the array of control input vector and Lagrange multiplier with respect to the equality constrain...
Definition: multiple_shooting_cgmres_solver.hpp:223
void set_uc(const MatrixBase< VectorType > &uc)
Sets the control input vector and Lagrange multiplier with respect to the equality constraints.
Definition: multiple_shooting_cgmres_solver.hpp:129
const std::array< Vector< nx >, N+1 > & xopt() const
Getter of the optimal solution.
Definition: multiple_shooting_cgmres_solver.hpp:371
const std::array< Vector< nub >, N > & muopt() const
Getter of the optimal solution.
Definition: multiple_shooting_cgmres_solver.hpp:389
detail::MatrixFreeGMRES< ContinuationGMRES_, kmax > MatrixFreeGMRES_
Definition: multiple_shooting_cgmres_solver.hpp:60
const std::array< Vector< nub >, N > & dummyopt() const
Getter of the optimal solution.
Definition: multiple_shooting_cgmres_solver.hpp:383
MultipleShootingCGMRESSolver(const OCP &ocp, const Horizon &horizon, const SolverSettings &settings)
Constructs the multiple-shooting C/GMRES solver.
Definition: multiple_shooting_cgmres_solver.hpp:68
void set_u(const MatrixBase< VectorType > &u)
Sets the control input vector.
Definition: multiple_shooting_cgmres_solver.hpp:110
void init_dummy_mu()
Initializes the dummy input vectors and Lagrange multipliers with respect to the control input bounds...
Definition: multiple_shooting_cgmres_solver.hpp:350
void set_mu(const MatrixBase< VectorType > &mu)
Sets the Lagrange multiplier with respect to the control input bounds constraint.
Definition: multiple_shooting_cgmres_solver.hpp:189
void set_u_array(const T &u_array)
Sets the array of control input vector.
Definition: multiple_shooting_cgmres_solver.hpp:203
static constexpr int dim
Dimension of the linear problem solved by the GMRES solver.
Definition: multiple_shooting_cgmres_solver.hpp:56
void init_x_lmd(const Scalar t, const MatrixBase< VectorType > &x)
Initializes the state vectors and costate vectors by simulating the system dynamics and system costat...
Definition: multiple_shooting_cgmres_solver.hpp:339
static constexpr int nuc
Dimension of the concatenation of the control input and equality constraints.
Definition: multiple_shooting_cgmres_solver.hpp:46
static constexpr int nx
Dimension of the state.
Definition: multiple_shooting_cgmres_solver.hpp:31
const std::array< Vector< nu >, N > & uopt() const
Getter of the optimal solution.
Definition: multiple_shooting_cgmres_solver.hpp:359
A timer for benchmarks.
Definition: timer.hpp:50
void tick()
Start timer (tick).
Definition: timer.hpp:71
TimingProfile getProfile() const
Get timing result as TimingProfile.
Definition: timer.hpp:90
void tock()
Stop the timer (tock).
Definition: timer.hpp:78
void synchronize_ocp()
Definition: continuation_gmres_condensing.hpp:302
void retrive_x(const Scalar t, const MatrixBase< VectorType > &x0, const Vector< dim > &solution, std::array< Vector< nx >, N+1 > &x)
Definition: continuation_gmres_condensing.hpp:256
void eval_fonc(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, const std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu)
Definition: continuation_gmres_condensing.hpp:87
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)
Definition: continuation_gmres_condensing.hpp:264
void retrive_dummy(const Vector< dim > &solution, std::array< Vector< nub >, N > &dummy, const std::array< Vector< nub >, N > &mu, const Scalar min_dummy)
Definition: continuation_gmres_condensing.hpp:272
Scalar optError() const
Definition: continuation_gmres_condensing.hpp:67
const NLP & get_nlp() const
Definition: continuation_gmres_condensing.hpp:300
void retrive_mu(const Vector< dim > &solution, const std::array< Vector< nub >, N > &dummy, std::array< Vector< nub >, N > &mu)
Definition: continuation_gmres_condensing.hpp:287
Definition: multiple_shooting_nlp.hpp:16
const OCP & ocp() const
Definition: multiple_shooting_nlp.hpp:184
const Horizon & horizon() const
Definition: multiple_shooting_nlp.hpp:186
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
Settings of solvers.
Definition: solver_settings.hpp:14
Scalar min_dummy
The minimum value of the dummy inputs. Mainly used in MultipleShootingCGMRESSolver....
Definition: solver_settings.hpp:58
Scalar finite_difference_epsilon
Epsilon of the finite difference approximation. Must be positive. Default value is 1....
Definition: solver_settings.hpp:33
Scalar sampling_time
The sampling time of MPC and used in SingleShootingCGMRESSolver and MultipleShootingCGMRESSolver....
Definition: solver_settings.hpp:40
Scalar zeta
The stabilization parameter of the continuation method. Typical value is the reciprocal of SolverSett...
Definition: solver_settings.hpp:49
size_t verbose_level
Verbose level. 0: no printings. 1-2: print some things. Default is 0.
Definition: solver_settings.hpp:63
bool profile_solver
If true, a solver profile is taken.
Definition: solver_settings.hpp:68
A profile of the timing benchmark.
Definition: timer.hpp:16