1#ifndef CGMRES__SINGLE_SHOOTING_NLP_HPP_
2#define CGMRES__SINGLE_SHOOTING_NLP_HPP_
15template <
class OCP,
int N>
18 static constexpr int nx = OCP::nx;
19 static constexpr int nu = OCP::nu;
20 static constexpr int nc = OCP::nc;
22 static constexpr int nub = OCP::nub;
29 static_assert(OCP::nx > 0);
30 static_assert(OCP::nu > 0);
31 static_assert(OCP::nc >= 0);
32 static_assert(OCP::nub >= 0);
42 template <
typename VectorType>
50 ocp_.eval_f(t, x_[0].data(), solution.template head<nuc>().data(), dx_.data());
51 x_[1] = x_[0] + dt * dx_;
52 for (
size_t i=1; i<N; ++i) {
53 const int inucb2 = i * (
nuc + 2 *
nub);
54 ocp_.eval_f(t+i*dt, x_[i].data(), solution.template segment<nuc>(inucb2).data(), dx_.data());
55 x_[i+1] = x_[i] + dt * dx_;
58 ocp_.eval_phix(t+T, x_[N].data(), lmd_[N].data());
59 for (
size_t i=N-1; i>=1; --i) {
60 const int inucb2 = i * (
nuc + 2 *
nub);
61 ocp_.eval_hx(t+i*dt, x_[i].data(), solution.template segment<nuc>(inucb2).data(),
62 lmd_[i+1].data(), dx_.data());
63 lmd_[i] = lmd_[i+1] + dt * dx_;
66 ocp_.eval_hu(t, x_[0].data(), solution.template head<nuc>().data(), lmd_[1].data(),
67 fonc_hu.template head<nuc>().data());
68 for (
size_t i=1; i<N; ++i) {
69 const int inucb2 = i * (
nuc + 2 *
nub);
70 ocp_.eval_hu(t+i*dt, x_[i].data(), solution.template segment<nuc>(inucb2).data(),
71 lmd_[i+1].data(), fonc_hu.template segment<nuc>(inucb2).data());
73 if constexpr (
nub > 0) {
74 for (
size_t i=0; i<N; ++i) {
75 const int inucb2 = i * (
nuc + 2 *
nub);
76 const auto uc = solution.template segment<nuc>(inucb2);
77 const auto dummy = solution.template segment<nub>(inucb2+
nuc);
78 const auto mu = solution.template segment<nub>(inucb2+
nuc+
nub);
79 auto fonc_huc = fonc_hu.template segment<nuc>(inucb2);
80 auto fonc_hdummy = fonc_hu.template segment<nub>(inucb2+
nuc);
81 auto fonc_hmu = fonc_hu.template segment<nub>(inucb2+
nuc+
nub);
90 if constexpr (
nub > 0) {
91 for (
size_t i=0; i<N; ++i) {
92 const int inucb2 = i * (
nuc + 2 *
nub);
93 const auto uc = solution.template segment<nuc>(inucb2);
94 auto dummy = solution.template segment<nub>(inucb2+
nuc);
95 const auto mu = solution.template segment<nub>(inucb2+
nuc+
nub);
96 auto fonc_hmu = fonc_hu.template segment<nub>(inucb2+
nuc+
nub);
99 dummy.array() = fonc_hmu.array().abs().sqrt();
105 if constexpr (
nub > 0) {
106 for (
size_t i=0; i<N; ++i) {
107 const int inucb2 = i * (
nuc + 2 *
nub);
108 const auto uc = solution.template segment<nuc>(inucb2);
109 const auto dummy = solution.template segment<nub>(inucb2+
nuc);
110 auto mu = solution.template segment<nub>(inucb2+
nuc+
nub);
111 auto fonc_hdummy = fonc_hu.template segment<nub>(inucb2+
nuc);
114 mu.array() = - fonc_hdummy.array() / (2.0 * dummy.array());
121 const OCP&
ocp()
const {
return ocp_; }
125 const std::array<Vector<nx>, N+1>&
x()
const {
return x_; }
127 const std::array<Vector<nx>, N+1>&
lmd()
const {
return lmd_; }
129 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135 std::array<Vector<nx>, N+1> x_, lmd_;
Horizon of MPC.
Definition: horizon.hpp:18
Scalar T(const Scalar t) const
Gets the length of the horizon.
Definition: horizon.hpp:50
Definition: single_shooting_nlp.hpp:16
static constexpr int nc
Definition: single_shooting_nlp.hpp:20
SingleShootingNLP()=default
~SingleShootingNLP()=default
void synchronize_ocp()
Definition: single_shooting_nlp.hpp:119
void retrive_dummy(Vector< dim > &solution, Vector< dim > &fonc_hu, const Scalar min_dummy)
Definition: single_shooting_nlp.hpp:89
SingleShootingNLP(const OCP &ocp, const Horizon &horizon)
Definition: single_shooting_nlp.hpp:25
static constexpr int nx
Definition: single_shooting_nlp.hpp:18
static constexpr int dim
Definition: single_shooting_nlp.hpp:23
const OCP & ocp() const
Definition: single_shooting_nlp.hpp:121
const std::array< Vector< nx >, N+1 > & x() const
Definition: single_shooting_nlp.hpp:125
const Horizon & horizon() const
Definition: single_shooting_nlp.hpp:123
void eval_fonc_hu(const Scalar t, const MatrixBase< VectorType > &x, const Vector< dim > &solution, Vector< dim > &fonc_hu)
Definition: single_shooting_nlp.hpp:43
static constexpr int nub
Definition: single_shooting_nlp.hpp:22
static constexpr int nuc
Definition: single_shooting_nlp.hpp:21
void retrive_mu(Vector< dim > &solution, Vector< dim > &fonc_hu)
Definition: single_shooting_nlp.hpp:104
const std::array< Vector< nx >, N+1 > & lmd() const
Definition: single_shooting_nlp.hpp:127
static constexpr int nu
Definition: single_shooting_nlp.hpp:19
void clip_dummy(const MatrixBase< VectorType > &dummy, const Scalar min)
Definition: control_input_bounds.hpp:127
void eval_hmu(const OCP &ocp, const MatrixBase< VectorType1 > &u, const MatrixBase< VectorType2 > &dummy, const MatrixBase< VectorType3 > &mu, const MatrixBase< VectorType4 > &hmu)
Definition: control_input_bounds.hpp:47
void eval_hdummy(const OCP &ocp, const MatrixBase< VectorType1 > &u, const MatrixBase< VectorType2 > &dummy, const MatrixBase< VectorType3 > &mu, const MatrixBase< VectorType4 > &hdummy)
Definition: control_input_bounds.hpp:32
void eval_hu(const OCP &ocp, const MatrixBase< VectorType1 > &u, const MatrixBase< VectorType2 > &dummy, const MatrixBase< VectorType3 > &mu, const MatrixBase< VectorType4 > &hu)
Definition: control_input_bounds.hpp:15
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