autogenu-jupyter
An automatic code generator and the continuation/GMRES (C/GMRES) based numerical solvers for nonlinear MPC
Loading...
Searching...
No Matches
control_input_bounds.hpp
Go to the documentation of this file.
1#ifndef CGMRES__CONTROL_INPUT_BOUNDS_HPP_
2#define CGMRES__CONTROL_INPUT_BOUNDS_HPP_
3
4#include <array>
5
6#include "cgmres/types.hpp"
7
9
10namespace cgmres {
11namespace detail {
12namespace ubounds {
13
14template <typename OCP, typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
15void eval_hu(const OCP& ocp, const MatrixBase<VectorType1>& u,
16 const MatrixBase<VectorType2>& dummy,
17 const MatrixBase<VectorType3>& mu,
18 const MatrixBase<VectorType4>& hu) {
19 constexpr int nub = OCP::nub;
20 if constexpr (nub > 0) {
21 assert(dummy.size() == nub);
22 assert(mu.size() == nub);
23 for (int i=0; i<nub; ++i) {
24 const auto ui = OCP::ubound_indices[i];
25 CGMRES_EIGEN_CONST_CAST(VectorType4, hu).coeffRef(ui)
26 += mu.coeff(i) * (2.0*u.coeff(ui) - ocp.umin[i] - ocp.umax[i]);
27 }
28 }
29}
30
31template <typename OCP, typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
32void eval_hdummy(const OCP& ocp, const MatrixBase<VectorType1>& u,
33 const MatrixBase<VectorType2>& dummy,
34 const MatrixBase<VectorType3>& mu,
35 const MatrixBase<VectorType4>& hdummy) {
36 constexpr int nub = OCP::nub;
37 if constexpr (nub > 0) {
38 assert(dummy.size() == nub);
39 assert(mu.size() == nub);
40 assert(hdummy.size() == nub);
41 CGMRES_EIGEN_CONST_CAST(VectorType4, hdummy).array()
42 = 2.0 * mu.array() * dummy.array() - Map<const Vector<nub>>(ocp.dummy_weight.data()).array();
43 }
44}
45
46template <typename OCP, typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
47void eval_hmu(const OCP& ocp, const MatrixBase<VectorType1>& u,
48 const MatrixBase<VectorType2>& dummy,
49 const MatrixBase<VectorType3>& mu,
50 const MatrixBase<VectorType4>& hmu) {
51 constexpr int nub = OCP::nub;
52 if constexpr (nub > 0) {
53 assert(dummy.size() == nub);
54 assert(mu.size() == nub);
55 assert(hmu.size() == nub);
56 for (int i=0; i<nub; ++i) {
57 const auto ui = OCP::ubound_indices[i];
58 CGMRES_EIGEN_CONST_CAST(VectorType4, hmu).coeffRef(i)
59 = u.coeff(ui) * (u.coeff(ui) - ocp.umin[i] - ocp.umax[i])
60 + ocp.umin[i] * ocp.umax[i] + dummy.coeff(i) * dummy.coeff(i);
61 }
62 }
63}
64
65template <typename VectorType1, typename VectorType2, typename VectorType3,
66 typename VectorType4, typename VectorType5>
68 const MatrixBase<VectorType2>& mu,
69 const MatrixBase<VectorType3>& hdummy,
70 const MatrixBase<VectorType4>& hmu,
71 const MatrixBase<VectorType5>& hdummy_multiplied) {
72 CGMRES_EIGEN_CONST_CAST(VectorType5, hdummy_multiplied).array() = hmu.array() / (2.0 * dummy.array());
73}
74
75template <typename VectorType1, typename VectorType2, typename VectorType3,
76 typename VectorType4, typename VectorType5, typename VectorType6>
78 const MatrixBase<VectorType2>& mu,
79 const MatrixBase<VectorType3>& hdummy,
80 const MatrixBase<VectorType4>& hmu,
81 const MatrixBase<VectorType5>& hdummy_multiplied,
82 const MatrixBase<VectorType6>& hmu_multiplied) {
83 CGMRES_EIGEN_CONST_CAST(VectorType6, hmu_multiplied).array() = hdummy.array() / (2.0 * dummy.array())
84 - mu.array() * hdummy_multiplied.array() / dummy.array();
85}
86
87template <typename OCP, typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4, typename VectorType5>
88void retrive_dummy_update(const OCP& ocp,
90 const MatrixBase<VectorType2>& dummy,
91 const MatrixBase<VectorType3>& mu,
92 const MatrixBase<VectorType4>& u_update,
93 const MatrixBase<VectorType5>& dummy_update) {
94 constexpr int nub = OCP::nub;
95 if constexpr (nub > 0) {
96 assert(dummy.size() == nub);
97 assert(dummy_update.size() == nub);
98 for (int i=0; i<nub; ++i) {
99 const auto ui = OCP::ubound_indices[i];
100 CGMRES_EIGEN_CONST_CAST(VectorType5, dummy_update).coeffRef(ui)
101 = (2.0*u.coeff(ui) - ocp.umin[i] - ocp.umax[i]) * u_update.coeff(ui) / (2.0 * dummy.coeff(i));
102 }
103 }
104}
105
106template <typename OCP, typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4, typename VectorType5>
107void retrive_mu_update(const OCP& ocp,
108 const MatrixBase<VectorType1>& u,
109 const MatrixBase<VectorType2>& dummy,
110 const MatrixBase<VectorType3>& mu,
111 const MatrixBase<VectorType4>& u_update,
112 const MatrixBase<VectorType5>& mu_udpate) {
113 constexpr int nub = OCP::nub;
114 if constexpr (nub > 0) {
115 assert(dummy.size() == nub);
116 assert(mu_udpate.size() == nub);
117 for (int i=0; i<nub; ++i) {
118 const auto ui = OCP::ubound_indices[i];
119 CGMRES_EIGEN_CONST_CAST(VectorType5, mu_udpate).coeffRef(ui)
120 = - mu.coeff(i) * (2.0*u.coeff(ui) - ocp.umin[i] - ocp.umax[i]) * u_update.coeff(ui)
121 / (2.0 * dummy.coeff(i) * dummy.coeff(i));
122 }
123 }
124}
125
126template <typename VectorType>
127void clip_dummy(const MatrixBase<VectorType>& dummy, const Scalar min) {
128 assert(min >= 0.0);
129 const size_t nub = dummy.size();
130 for (size_t i=0; i<nub; ++i) {
131 CGMRES_EIGEN_CONST_CAST(VectorType, dummy).coeffRef(i) = std::max(dummy.coeff(i), min);
132 }
133}
134
135} // namespace ubounds
136} // namespace detail
137} // namespace cgmres
138
139#endif // CGMRES__CONTROL_INPUT_BOUNDS_HPP_
#define CGMRES_EIGEN_CONST_CAST(TYPE, OBJ)
Definition: macros.hpp:7
void retrive_mu_update(const OCP &ocp, const MatrixBase< VectorType1 > &u, const MatrixBase< VectorType2 > &dummy, const MatrixBase< VectorType3 > &mu, const MatrixBase< VectorType4 > &u_update, const MatrixBase< VectorType5 > &mu_udpate)
Definition: control_input_bounds.hpp:107
void retrive_dummy_update(const OCP &ocp, const MatrixBase< VectorType1 > &u, const MatrixBase< VectorType2 > &dummy, const MatrixBase< VectorType3 > &mu, const MatrixBase< VectorType4 > &u_update, const MatrixBase< VectorType5 > &dummy_update)
Definition: control_input_bounds.hpp:88
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 multiply_hmu_inv(const MatrixBase< VectorType1 > &dummy, const MatrixBase< VectorType2 > &mu, const MatrixBase< VectorType3 > &hdummy, const MatrixBase< VectorType4 > &hmu, const MatrixBase< VectorType5 > &hdummy_multiplied, const MatrixBase< VectorType6 > &hmu_multiplied)
Definition: control_input_bounds.hpp:77
void multiply_hdummy_inv(const MatrixBase< VectorType1 > &dummy, const MatrixBase< VectorType2 > &mu, const MatrixBase< VectorType3 > &hdummy, const MatrixBase< VectorType4 > &hmu, const MatrixBase< VectorType5 > &hdummy_multiplied)
Definition: control_input_bounds.hpp:67
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::Map< MatrixType > Map
Alias of Eigen::Map.
Definition: types.hpp:50
double Scalar
Alias of double.
Definition: types.hpp:11
Eigen::MatrixBase< MatrixType > MatrixBase
Alias of Eigen::MatrixBase.
Definition: types.hpp:29