1#ifndef CGMRES__MULTIPLE_SHOOTING_CGMRES_SOLVER_HPP_
2#define CGMRES__MULTIPLE_SHOOTING_CGMRES_SOLVER_HPP_
25template <
class OCP,
int N,
int kmax>
31 static constexpr int nx = OCP::nx;
36 static constexpr int nu = OCP::nu;
41 static constexpr int nc = OCP::nc;
51 static constexpr int nub = OCP::nub;
70 : continuation_gmres_(
MultipleShootingNLP_(ocp, horizon), settings.finite_difference_epsilon, settings.zeta),
79 if constexpr (
nub > 0) {
85 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver]: 'settings.finite_difference_epsilon' must be positive!");
88 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver]: 'settings.sampling_time' must be positive!");
90 if (settings.
zeta <= 0.0) {
91 throw std::invalid_argument(
"[ContinuationGMRESCondensing]: 'settings.zeta' must be positive!");
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));
114 for (
size_t i=0; i<N; ++i) {
117 for (
size_t i=0; i<N; ++i) {
118 ucopt_[i].template head<nu>() = u;
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));
133 for (
size_t i=0; i<N; ++i) {
134 uopt_[i] = uc.template head<nu>();
136 for (
size_t i=0; i<N; ++i) {
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));
151 for (
size_t i=1; i<=N; ++i) {
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));
165 for (
size_t i=1; i<=N; ++i) {
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));
179 for (
size_t i=0; i<N; ++i) {
180 dummyopt_[i] = dummy;
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));
193 for (
size_t i=0; i<N; ++i) {
202 template <
typename T>
204 if (u_array.size() != N) {
205 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::set_u_array]: 'u_array.size()' must be "+std::to_string(N));
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));
211 uopt_[i] = u_array[i];
212 ucopt_[i].template head<nu>() = u_array[i];
222 template <
typename T>
224 if (uc_array.size() != N) {
225 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::set_uc_array]: 'uc_array.size()' must be "+std::to_string(N));
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));
231 uopt_[i] = uc_array[i].template head<nu>();
232 ucopt_[i] = uc_array[i];
241 template <
typename T>
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));
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));
250 xopt_[i] = x_array[i];
258 template <
typename T>
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));
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));
267 lmdopt_[i] = lmd_array[i];
276 template <
typename T>
278 if (dummy_array.size() != N) {
279 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::set_dummy_array]: 'dummy_array.size()' must be "+std::to_string(N));
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));
285 dummyopt_[i] = dummy_array[i];
294 template <
typename T>
296 if (mu_array.size() != N) {
297 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::set_mu_array]: 'mu_array.size()' must be "+std::to_string(N));
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));
303 muopt_[i] = mu_array[i];
312 template <
typename VectorType>
314 if (x.size() !=
nx) {
315 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::init_x] x.size() must be " + std::to_string(
nx));
317 continuation_gmres_.
retrive_x(t, x, solution_, xopt_);
325 template <
typename VectorType>
327 if (x.size() !=
nx) {
328 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::init_lmd] x.size() must be " + std::to_string(
nx));
330 continuation_gmres_.
retrive_lmd(t, x, solution_, xopt_, lmdopt_);
338 template <
typename VectorType>
340 if (x.size() !=
nx) {
341 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::init_x_lmd] x.size() must be " + std::to_string(
nx));
343 continuation_gmres_.
retrive_x(t, x, solution_, xopt_);
344 continuation_gmres_.
retrive_lmd(t, x, solution_, xopt_, lmdopt_);
352 continuation_gmres_.
retrive_mu(solution_, dummyopt_, muopt_);
359 const std::array<Vector<nu>, N>&
uopt()
const {
return uopt_; }
365 const std::array<Vector<nuc>, N>&
ucopt()
const {
return ucopt_; }
371 const std::array<Vector<nx>, N+1>&
xopt()
const {
return xopt_; }
377 const std::array<Vector<nx>, N+1>&
lmdopt()
const {
return lmdopt_; }
383 const std::array<Vector<nub>, N>&
dummyopt()
const {
return dummyopt_; }
389 const std::array<Vector<nub>, N>&
muopt()
const {
return muopt_; }
403 template <
typename VectorType>
405 if (x.size() !=
nx) {
406 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::optError] x.size() must be " + std::to_string(
nx));
409 continuation_gmres_.
eval_fonc(t, x, solution_, xopt_, lmdopt_, dummyopt_, muopt_);
418 template <
typename VectorType>
420 if (x.size() !=
nx) {
421 throw std::invalid_argument(
"[MultipleShootingCGMRESSolver::update] x.size() must be " + std::to_string(
nx));
424 std::cout <<
"\n======================= update solution with C/GMRES =======================" << std::endl;
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_,
437 solution_.noalias() += settings_.
sampling_time * solution_update_;
443 std::cout <<
"opt error: " << opt_error << std::endl;
446 std::cout <<
"number of GMRES iter: " << gmres_iter <<
" (kmax: " << kmax <<
")" << std::endl;
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;
464 os << settings_ << std::endl;
473 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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_;
490 void setInnerSolution() {
491 for (
size_t i=0; i<N; ++i) {
492 solution_.template segment<nuc>(i*
nuc) = ucopt_[i];
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);
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