autogenu-jupyter
An automatic code generator and the continuation/GMRES (C/GMRES) based numerical solvers for nonlinear MPC
Loading...
Searching...
No Matches
ocp.hpp
Go to the documentation of this file.
1// This file was automatically generated by autogenu-jupyter (https://github.com/mayataka/autogenu-jupyter).
2// The autogenu-jupyter copyright holders make no ownership claim of its contents.
3
4#ifndef CGMRES__OCP_PENDUBOT_HPP_
5#define CGMRES__OCP_PENDUBOT_HPP_
6
7#define _USE_MATH_DEFINES
8
9#include <cmath>
10#include <array>
11#include <iostream>
12
13#include "cgmres/types.hpp"
14#include "cgmres/detail/macros.hpp"
15
16namespace cgmres {
17
22class OCP_pendubot {
23public:
24
28 static constexpr int nx = 4;
29
33 static constexpr int nu = 1;
34
38 static constexpr int nc = 0;
39
43 static constexpr int nh = 0;
44
48 static constexpr int nuc = nu + nc;
49
53 static constexpr int nub = 1;
54
55 double m1 = 0.2;
56 double m2 = 0.7;
57 double l1 = 0.3;
58 double l2 = 0.3;
59 double d1 = 0.15;
60 double d2 = 0.257;
61 double J1 = 0.006;
62 double J2 = 0.051;
63 double g = 9.80665;
64
65 std::array<double, 4> q = {1, 1, 0.1, 0.1};
66 std::array<double, 4> q_terminal = {1, 1, 0.1, 0.1};
67 std::array<double, 4> x_ref = {M_PI, 0, 0, 0};
68 std::array<double, 1> r = {0.1};
69
70 static constexpr std::array<int, nub> ubound_indices = {0};
71 std::array<double, nub> umin = {-5.0};
72 std::array<double, nub> umax = {5.0};
73 std::array<double, nub> dummy_weight = {0.1};
74
75 void disp(std::ostream& os) const {
76 os << "OCP_pendubot:" << std::endl;
77 os << " nx: " << nx << std::endl;
78 os << " nu: " << nu << std::endl;
79 os << " nc: " << nc << std::endl;
80 os << " nh: " << nh << std::endl;
81 os << " nuc: " << nuc << std::endl;
82 os << " nub: " << nub << std::endl;
83 os << std::endl;
84 os << " m1: " << m1 << std::endl;
85 os << " m2: " << m2 << std::endl;
86 os << " l1: " << l1 << std::endl;
87 os << " l2: " << l2 << std::endl;
88 os << " d1: " << d1 << std::endl;
89 os << " d2: " << d2 << std::endl;
90 os << " J1: " << J1 << std::endl;
91 os << " J2: " << J2 << std::endl;
92 os << " g: " << g << std::endl;
93 os << std::endl;
94 Eigen::IOFormat fmt(4, 0, ", ", "", "[", "]");
95 Eigen::IOFormat intfmt(1, 0, ", ", "", "[", "]");
96 os << " q: " << Map<const VectorX>(q.data(), q.size()).transpose().format(fmt) << std::endl;
97 os << " q_terminal: " << Map<const VectorX>(q_terminal.data(), q_terminal.size()).transpose().format(fmt) << std::endl;
98 os << " x_ref: " << Map<const VectorX>(x_ref.data(), x_ref.size()).transpose().format(fmt) << std::endl;
99 os << " r: " << Map<const VectorX>(r.data(), r.size()).transpose().format(fmt) << std::endl;
100 os << std::endl;
101 os << " ubound_indices: " << Map<const VectorXi>(ubound_indices.data(), ubound_indices.size()).transpose().format(intfmt) << std::endl;
102 os << " umin: " << Map<const VectorX>(umin.data(), umin.size()).transpose().format(fmt) << std::endl;
103 os << " umax: " << Map<const VectorX>(umax.data(), umax.size()).transpose().format(fmt) << std::endl;
104 os << " dummy_weight: " << Map<const VectorX>(dummy_weight.data(), dummy_weight.size()).transpose().format(fmt) << std::endl;
105 }
106
107 friend std::ostream& operator<<(std::ostream& os, const OCP_pendubot& ocp) {
108 ocp.disp(os);
109 return os;
110 }
111
112
117 void synchronize() {
118 }
119
129 void eval_f(const double t, const double* x, const double* u,
130 double* dx) const {
131 const double x0 = pow(l1, 2);
132 const double x1 = m2*x0;
133 const double x2 = l1*m2;
134 const double x3 = d2*x2;
135 const double x4 = x3*cos(x[1]);
136 const double x5 = pow(d2, 2);
137 const double x6 = J2 + m2*x5;
138 const double x7 = J1 + pow(d1, 2)*m1;
139 const double x8 = 1.0/(x1 + 2.0*x4 + x6 + x7);
140 const double x9 = d2*g*m2*sin(x[0] + x[1]);
141 const double x10 = d1*m1;
142 const double x11 = x10 + x2;
143 const double x12 = sin(x[0]);
144 const double x13 = sin(x[1]);
145 const double x14 = 2.0*x[1];
146 const double x15 = 0.5*l1;
147 const double x16 = pow(m2, 2)*x5;
148 const double x17 = x15*x16;
149 const double x18 = pow(x[2], 2.0);
150 const double x19 = x[2]*x[3];
151 const double x20 = pow(x[3], 2.0);
152 dx[0] = x[2];
153 dx[1] = x[3];
154 dx[2] = x8*(2.0*d2*l1*m2*x13*x[3]*(x[2] + 0.5*x[3]) - g*x11*x12 + u[0] - x9);
155 dx[3] = x8*(0.5*d2*g*l1*m2*x11*sin(x[0] - x[1]) + g*x12*(J2*x10 + m2*(J2*l1 + x10*x5) + x17) - g*x17*sin(x14 + x[0]) - u[0]*(x4 + x6) - x0*x16*(x18 + x19 + 0.5*x20)*sin(x14) - x13*x3*(2.0*J2*x19 + J2*x20 + m2*(x0*x18 + x5*pow(x[2] + x[3], 2.0)) + x18*(J2 + x7)) - x9*(0.5*x1 - x10*x15 + x7))/x6;
156
157 }
158
168 void eval_phix(const double t, const double* x, double* phix) const {
169 phix[0] = (1.0/2.0)*q_terminal[0]*(2*x[0] - 2*x_ref[0]);
170 phix[1] = (1.0/2.0)*q_terminal[1]*(2*x[1] - 2*x_ref[1]);
171 phix[2] = (1.0/2.0)*q_terminal[2]*(2*x[2] - 2*x_ref[2]);
172 phix[3] = (1.0/2.0)*q_terminal[3]*(2*x[3] - 2*x_ref[3]);
173
174 }
175
187 void eval_hx(const double t, const double* x, const double* u,
188 const double* lmd, double* hx) const {
189 const double x0 = x[0] + x[1];
190 const double x1 = d2*g;
191 const double x2 = m2*x1;
192 const double x3 = x2*cos(x0);
193 const double x4 = d1*m1;
194 const double x5 = l1*m2;
195 const double x6 = x4 + x5;
196 const double x7 = cos(x[0]);
197 const double x8 = pow(l1, 2);
198 const double x9 = m2*x8;
199 const double x10 = cos(x[1]);
200 const double x11 = d2*x5;
201 const double x12 = x10*x11;
202 const double x13 = pow(d2, 2);
203 const double x14 = m2*x13;
204 const double x15 = J2 + x14;
205 const double x16 = pow(d1, 2)*m1;
206 const double x17 = J1 + x16;
207 const double x18 = 1.0/(2.0*x12 + x15 + x17 + x9);
208 const double x19 = lmd[2]*x18;
209 const double x20 = 0.5*l1;
210 const double x21 = pow(m2, 2)*x13;
211 const double x22 = x20*x21;
212 const double x23 = 2.0*x[1];
213 const double x24 = x23 + x[0];
214 const double x25 = g*cos(x24);
215 const double x26 = x[0] - x[1];
216 const double x27 = 0.5*x1*x5*x6*cos(x26);
217 const double x28 = 0.5*x9;
218 const double x29 = x17 - x20*x4 + x28;
219 const double x30 = x29*x3;
220 const double x31 = J2*x4 + m2*(J2*l1 + x13*x4) + x22;
221 const double x32 = lmd[3]/x15;
222 const double x33 = x18*x32;
223 const double x34 = x[2] + 0.5*x[3];
224 const double x35 = x2*sin(x0);
225 const double x36 = sin(x[0]);
226 const double x37 = sin(x[1]);
227 const double x38 = x11*x37;
228 const double x39 = 0.5*x38/pow(0.5*J1 + 0.5*J2 + x12 + 0.5*x14 + 0.5*x16 + x28, 2);
229 const double x40 = pow(x[2], 2.0);
230 const double x41 = x[2]*x[3];
231 const double x42 = pow(x[3], 2.0);
232 const double x43 = x40 + x41 + 0.5*x42;
233 const double x44 = 2.0*J2;
234 const double x45 = J2 + x17;
235 const double x46 = x[2] + x[3];
236 const double x47 = J2*x42 + m2*(x13*pow(x46, 2.0) + x40*x8) + x40*x45 + x41*x44;
237 const double x48 = x21*x8*sin(x23);
238 const double x49 = x38*x[3];
239 const double x50 = 2.0*pow(x[2], 1.0);
240 const double x51 = 2.0*pow(x46, 1.0);
241 const double x52 = pow(x[3], 1.0);
242 hx[0] = (1.0/2.0)*q[0]*(2*x[0] - 2*x_ref[0]) + x19*(-g*x6*x7 - x3) + x33*(g*x31*x7 - x22*x25 + x27 - x30);
243 hx[1] = lmd[2]*x39*(2.0*d2*l1*m2*x34*x37*x[3] - g*x36*x6 + u[0] - x35) + (1.0/2.0)*q[1]*(2*x[1] - 2*x_ref[1]) + x19*(2.0*d2*l1*m2*x10*x34*x[3] - x3) + x32*x39*(0.5*d2*g*l1*m2*x6*sin(x26) - g*x22*sin(x24) + g*x31*x36 - u[0]*(x12 + x15) - x29*x35 - x38*x47 - x43*x48) + x33*(d2*l1*m2*u[0]*x37 - 1.0*l1*x21*x25 - x12*x47 - 2.0*x21*x43*x8*cos(x23) - x27 - x30);
244 hx[2] = lmd[0] + (1.0/2.0)*q[2]*(2*x[2] - 2*x_ref[2]) + 2.0*x19*x49 + x33*(-x38*(m2*(x13*x51 + x50*x8) + x44*x[3] + x45*x50) - x48*(x50 + x[3]));
245 hx[3] = lmd[1] + (1.0/2.0)*q[3]*(2*x[3] - 2*x_ref[3]) + x19*(2.0*x34*x38 + 1.0*x49) + x33*(-x38*(x14*x51 + x44*x52 + x44*x[2]) - x48*(1.0*x52 + x[2]));
246
247 }
248
260 void eval_hu(const double t, const double* x, const double* u,
261 const double* lmd, double* hu) const {
262 const double x0 = d2*l1*m2*cos(x[1]);
263 const double x1 = J2 + pow(d2, 2)*m2;
264 const double x2 = 1.0/(J1 + pow(d1, 2)*m1 + pow(l1, 2)*m2 + 2.0*x0 + x1);
265 hu[0] = lmd[2]*x2 + lmd[3]*x2*(-x0 - x1)/x1 + r[0]*u[0];
266
267 }
268
276 template <typename VectorType1, typename VectorType2, typename VectorType3>
277 void eval_f(const double t, const MatrixBase<VectorType1>& x,
278 const MatrixBase<VectorType2>& u,
279 const MatrixBase<VectorType3>& dx) const {
280 if (x.size() != nx) {
281 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
282 }
283 if (u.size() != nu) {
284 throw std::invalid_argument("[OCP]: u.size() must be " + std::to_string(nu));
285 }
286 if (dx.size() != nx) {
287 throw std::invalid_argument("[OCP]: dx.size() must be " + std::to_string(nx));
288 }
289 eval_f(t, x.derived().data(), u.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType3, dx).data());
290 }
291
299 template <typename VectorType1, typename VectorType2>
300 void eval_phix(const double t, const MatrixBase<VectorType1>& x,
301 const MatrixBase<VectorType2>& phix) const {
302 if (x.size() != nx) {
303 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
304 }
305 if (phix.size() != nx) {
306 throw std::invalid_argument("[OCP]: phix.size() must be " + std::to_string(nx));
307 }
308 eval_phix(t, x.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType2, phix).data());
309 }
310
320 template <typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
321 void eval_hx(const double t, const MatrixBase<VectorType1>& x,
322 const MatrixBase<VectorType2>& uc,
323 const MatrixBase<VectorType3>& lmd,
324 const MatrixBase<VectorType4>& hx) const {
325 if (x.size() != nx) {
326 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
327 }
328 if (uc.size() != nuc) {
329 throw std::invalid_argument("[OCP]: uc.size() must be " + std::to_string(nuc));
330 }
331 if (lmd.size() != nx) {
332 throw std::invalid_argument("[OCP]: lmd.size() must be " + std::to_string(nx));
333 }
334 if (hx.size() != nuc) {
335 throw std::invalid_argument("[OCP]: hx.size() must be " + std::to_string(nx));
336 }
337 eval_hx(t, x.derived().data(), uc.derived().data(), lmd.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType4, hx).data());
338 }
339
349 template <typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
350 void eval_hu(const double t, const MatrixBase<VectorType1>& x,
351 const MatrixBase<VectorType2>& uc,
352 const MatrixBase<VectorType3>& lmd,
353 const MatrixBase<VectorType4>& hu) const {
354 if (x.size() != nx) {
355 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
356 }
357 if (uc.size() != nuc) {
358 throw std::invalid_argument("[OCP]: uc.size() must be " + std::to_string(nuc));
359 }
360 if (lmd.size() != nx) {
361 throw std::invalid_argument("[OCP]: lmd.size() must be " + std::to_string(nx));
362 }
363 if (hu.size() != nuc) {
364 throw std::invalid_argument("[OCP]: hu.size() must be " + std::to_string(nuc));
365 }
366 eval_hu(t, x.derived().data(), uc.derived().data(), lmd.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType4, hu).data());
367 }
368
369};
370
371} // namespace cgmres
372
373#endif // CGMRES_OCP_HPP_
cgmres::OCP_pendubot
Definition of the optimal control problem (OCP) of pendubot.
Definition: ocp.hpp:22
cgmres::OCP_pendubot::nh
static constexpr int nh
Dimension of the Fischer-Burmeister function (already counded in nc).
Definition: ocp.hpp:43
cgmres::OCP_pendubot::operator<<
friend std::ostream & operator<<(std::ostream &os, const OCP_pendubot &ocp)
Definition: ocp.hpp:107
cgmres::OCP_pendubot::eval_hx
void eval_hx(const double t, const double *x, const double *u, const double *lmd, double *hx) const
Computes the partial derivative of the Hamiltonian with respect to state, i.e., hx = dH/dx(t,...
Definition: ocp.hpp:187
cgmres::OCP_pendubot::disp
void disp(std::ostream &os) const
Definition: ocp.hpp:75
cgmres::OCP_pendubot::umax
std::array< double, nub > umax
Definition: ocp.hpp:72
cgmres::OCP_pendubot::d1
double d1
Definition: ocp.hpp:59
cgmres::OCP_pendubot::nu
static constexpr int nu
Dimension of the control input.
Definition: ocp.hpp:33
cgmres::OCP_pendubot::l2
double l2
Definition: ocp.hpp:58
cgmres::OCP_pendubot::nuc
static constexpr int nuc
Dimension of the concatenation of the control input and equality constraints.
Definition: ocp.hpp:48
cgmres::OCP_pendubot::eval_hu
void eval_hu(const double t, const MatrixBase< VectorType1 > &x, const MatrixBase< VectorType2 > &uc, const MatrixBase< VectorType3 > &lmd, const MatrixBase< VectorType4 > &hu) const
Computes the partial derivative of the Hamiltonian with respect to control input and the equality con...
Definition: ocp.hpp:350
cgmres::OCP_pendubot::eval_hx
void eval_hx(const double t, const MatrixBase< VectorType1 > &x, const MatrixBase< VectorType2 > &uc, const MatrixBase< VectorType3 > &lmd, const MatrixBase< VectorType4 > &hx) const
Computes the partial derivative of the Hamiltonian with respect to the state, i.e....
Definition: ocp.hpp:321
cgmres::OCP_pendubot::dummy_weight
std::array< double, nub > dummy_weight
Definition: ocp.hpp:73
cgmres::OCP_pendubot::g
double g
Definition: ocp.hpp:63
cgmres::OCP_pendubot::eval_phix
void eval_phix(const double t, const double *x, double *phix) const
Computes the partial derivative of terminal cost with respect to state, i.e., phix = dphi/dx(t,...
Definition: ocp.hpp:168
cgmres::OCP_pendubot::r
std::array< double, 1 > r
Definition: ocp.hpp:68
cgmres::OCP_pendubot::q
std::array< double, 4 > q
Definition: ocp.hpp:65
cgmres::OCP_pendubot::x_ref
std::array< double, 4 > x_ref
Definition: ocp.hpp:67
cgmres::OCP_pendubot::m1
double m1
Definition: ocp.hpp:55
cgmres::OCP_pendubot::eval_f
void eval_f(const double t, const double *x, const double *u, double *dx) const
Computes the state equation dx = f(t, x, u).
Definition: ocp.hpp:129
cgmres::OCP_pendubot::synchronize
void synchronize()
Synchrozies the internal parameters of this OCP with the external references. This method is called a...
Definition: ocp.hpp:117
cgmres::OCP_pendubot::umin
std::array< double, nub > umin
Definition: ocp.hpp:71
cgmres::OCP_pendubot::m2
double m2
Definition: ocp.hpp:56
cgmres::OCP_pendubot::nub
static constexpr int nub
Dimension of the bound constraints on the control input.
Definition: ocp.hpp:53
cgmres::OCP_pendubot::d2
double d2
Definition: ocp.hpp:60
cgmres::OCP_pendubot::q_terminal
std::array< double, 4 > q_terminal
Definition: ocp.hpp:66
cgmres::OCP_pendubot::l1
double l1
Definition: ocp.hpp:57
cgmres::OCP_pendubot::J1
double J1
Definition: ocp.hpp:61
cgmres::OCP_pendubot::J2
double J2
Definition: ocp.hpp:62
cgmres::OCP_pendubot::ubound_indices
static constexpr std::array< int, nub > ubound_indices
Definition: ocp.hpp:70
cgmres::OCP_pendubot::eval_phix
void eval_phix(const double t, const MatrixBase< VectorType1 > &x, const MatrixBase< VectorType2 > &phix) const
Computes the partial derivative of terminal cost with respect to state, i.e., phix = dphi/dx(t,...
Definition: ocp.hpp:300
cgmres::OCP_pendubot::nx
static constexpr int nx
Dimension of the state.
Definition: ocp.hpp:28
cgmres::OCP_pendubot::nc
static constexpr int nc
Dimension of the equality constraints.
Definition: ocp.hpp:38
cgmres::OCP_pendubot::eval_hu
void eval_hu(const double t, const double *x, const double *u, const double *lmd, double *hu) const
Computes the partial derivative of the Hamiltonian with respect to control input and the equality con...
Definition: ocp.hpp:260
cgmres::OCP_pendubot::eval_f
void eval_f(const double t, const MatrixBase< VectorType1 > &x, const MatrixBase< VectorType2 > &u, const MatrixBase< VectorType3 > &dx) const
Computes the state equation dx = f(t, x, u).
Definition: ocp.hpp:277
CGMRES_EIGEN_CONST_CAST
#define CGMRES_EIGEN_CONST_CAST(TYPE, OBJ)
Definition: macros.hpp:7
cgmres
Definition: continuation_gmres.hpp:11
cgmres::Map
Eigen::Map< MatrixType > Map
Alias of Eigen::Map.
Definition: types.hpp:50
cgmres::MatrixBase
Eigen::MatrixBase< MatrixType > MatrixBase
Alias of Eigen::MatrixBase.
Definition: types.hpp:29