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_HEXACOPTER_HPP_
5#define CGMRES__OCP_HEXACOPTER_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_hexacopter {
23public:
24
28 static constexpr int nx = 12;
29
33 static constexpr int nu = 6;
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 = 6;
54
55 double m = 1.44;
56 double l = 0.23;
57 double k = 1.6e-09;
58 double Ixx = 0.0348;
59 double Iyy = 0.0459;
60 double Izz = 0.0977;
61 double gamma = 0.01;
62 double g = 9.80665;
63 double z_ref = 5;
64
65 std::array<double, 12> q = {1, 1, 1, 0.01, 0.01, 0, 0.01, 0.01, 0.01, 0.1, 0.1, 0.001};
66 std::array<double, 12> q_terminal = {1, 1, 1, 0.01, 0.01, 0, 0.01, 0.01, 0.01, 0.1, 0.1, 0.001};
67 std::array<double, 6> r = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
68
69 static constexpr std::array<int, nub> ubound_indices = {0, 1, 2, 3, 4, 5};
70 std::array<double, nub> umin = {0.144, 0.144, 0.144, 0.144, 0.144, 0.144};
71 std::array<double, nub> umax = {6.0, 6.0, 6.0, 6.0, 6.0, 6.0};
72 std::array<double, nub> dummy_weight = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1};
73
74 void disp(std::ostream& os) const {
75 os << "OCP_hexacopter:" << std::endl;
76 os << " nx: " << nx << std::endl;
77 os << " nu: " << nu << std::endl;
78 os << " nc: " << nc << std::endl;
79 os << " nh: " << nh << std::endl;
80 os << " nuc: " << nuc << std::endl;
81 os << " nub: " << nub << std::endl;
82 os << std::endl;
83 os << " m: " << m << std::endl;
84 os << " l: " << l << std::endl;
85 os << " k: " << k << std::endl;
86 os << " Ixx: " << Ixx << std::endl;
87 os << " Iyy: " << Iyy << std::endl;
88 os << " Izz: " << Izz << std::endl;
89 os << " gamma: " << gamma << std::endl;
90 os << " g: " << g << std::endl;
91 os << " z_ref: " << z_ref << std::endl;
92 os << std::endl;
93 Eigen::IOFormat fmt(4, 0, ", ", "", "[", "]");
94 Eigen::IOFormat intfmt(1, 0, ", ", "", "[", "]");
95 os << " q: " << Map<const VectorX>(q.data(), q.size()).transpose().format(fmt) << std::endl;
96 os << " q_terminal: " << Map<const VectorX>(q_terminal.data(), q_terminal.size()).transpose().format(fmt) << std::endl;
97 os << " r: " << Map<const VectorX>(r.data(), r.size()).transpose().format(fmt) << std::endl;
98 os << std::endl;
99 os << " ubound_indices: " << Map<const VectorXi>(ubound_indices.data(), ubound_indices.size()).transpose().format(intfmt) << std::endl;
100 os << " umin: " << Map<const VectorX>(umin.data(), umin.size()).transpose().format(fmt) << std::endl;
101 os << " umax: " << Map<const VectorX>(umax.data(), umax.size()).transpose().format(fmt) << std::endl;
102 os << " dummy_weight: " << Map<const VectorX>(dummy_weight.data(), dummy_weight.size()).transpose().format(fmt) << std::endl;
103 }
104
105 friend std::ostream& operator<<(std::ostream& os, const OCP_hexacopter& ocp) {
106 ocp.disp(os);
107 return os;
108 }
109
110
115 void synchronize() {
116 }
117
127 void eval_f(const double t, const double* x, const double* u,
128 double* dx) const {
129 const double x0 = sin(x[3]);
130 const double x1 = sin(x[5]);
131 const double x2 = cos(x[5]);
132 const double x3 = cos(x[3]);
133 const double x4 = sin(x[4]);
134 const double x5 = 1.0/m;
135 const double x6 = u[0] + u[2] + u[4];
136 const double x7 = u[1] + u[3] + u[5] + x6;
137 const double x8 = x5*x7;
138 const double x9 = 1.0/Ixx;
139 const double x10 = -Izz;
140 const double x11 = (1.0/2.0)*u[0];
141 const double x12 = 1.0/Iyy;
142 const double x13 = sqrt(3);
143 const double x14 = 1.0/Izz;
144 dx[0] = x[6];
145 dx[1] = x[7];
146 dx[2] = x[8];
147 dx[3] = x[9];
148 dx[4] = x[10];
149 dx[5] = x[11];
150 dx[6] = x8*(x0*x1 + x2*x3*x4);
151 dx[7] = x8*(-x0*x2 + x1*x3*x4);
152 dx[8] = -g + x3*x5*x7*cos(x[4]);
153 dx[9] = l*x9*(-u[1] - 1.0/2.0*u[2] + (1.0/2.0)*u[3] + u[4] + (1.0/2.0)*u[5] - x11) + x9*x[10]*x[11]*(Iyy + x10);
154 dx[10] = l*x12*((1.0/2.0)*u[2]*x13 + (1.0/2.0)*u[3]*x13 - 1.0/2.0*u[5]*x13 - x11*x13) + x12*x[11]*x[9]*(-Ixx - x10);
155 dx[11] = x14*x[10]*x[9]*(Ixx - Iyy) + x14*(-gamma*x[11] + k*(u[1] + u[3] + u[5] - x6));
156
157 }
158
168 void eval_phix(const double t, const double* x, double* phix) const {
169 const double x0 = 2*t;
170 const double x1 = sin(x0);
171 const double x2 = cos(x0);
172 phix[0] = (1.0/2.0)*q_terminal[0]*(-2*x1 + 2*x[0]);
173 phix[1] = (1.0/2.0)*q_terminal[1]*(2*x2 + 2*x[1] - 2);
174 phix[2] = (1.0/2.0)*q_terminal[2]*(2*x[2] - 2*z_ref - 4*sin(t));
175 phix[3] = q_terminal[3]*x[3];
176 phix[4] = q_terminal[4]*x[4];
177 phix[5] = q_terminal[5]*x[5];
178 phix[6] = (1.0/2.0)*q_terminal[6]*(-4*x2 + 2*x[6]);
179 phix[7] = (1.0/2.0)*q_terminal[7]*(-4*x1 + 2*x[7]);
180 phix[8] = (1.0/2.0)*q_terminal[8]*(2*x[8] - 4*cos(t));
181 phix[9] = q_terminal[9]*x[9];
182 phix[10] = q_terminal[10]*x[10];
183 phix[11] = q_terminal[11]*x[11];
184
185 }
186
198 void eval_hx(const double t, const double* x, const double* u,
199 const double* lmd, double* hx) const {
200 const double x0 = 2*t;
201 const double x1 = sin(x0);
202 const double x2 = cos(x0);
203 const double x3 = sin(x[3]);
204 const double x4 = cos(x[4]);
205 const double x5 = (u[0] + u[1] + u[2] + u[3] + u[4] + u[5])/m;
206 const double x6 = lmd[8]*x5;
207 const double x7 = sin(x[5]);
208 const double x8 = cos(x[3]);
209 const double x9 = sin(x[4]);
210 const double x10 = cos(x[5]);
211 const double x11 = x10*x3;
212 const double x12 = lmd[6]*x5;
213 const double x13 = x10*x8;
214 const double x14 = x3*x7;
215 const double x15 = lmd[7]*x5;
216 const double x16 = x7*x8;
217 const double x17 = -Izz;
218 const double x18 = lmd[10]*(-Ixx - x17)/Iyy;
219 const double x19 = lmd[11]/Izz;
220 const double x20 = x19*(Ixx - Iyy);
221 const double x21 = lmd[9]*(Iyy + x17)/Ixx;
222 hx[0] = (1.0/2.0)*q[0]*(-2*x1 + 2*x[0]);
223 hx[1] = (1.0/2.0)*q[1]*(2*x2 + 2*x[1] - 2);
224 hx[2] = (1.0/2.0)*q[2]*(2*x[2] - 2*z_ref - 4*sin(t));
225 hx[3] = q[3]*x[3] + x12*(-x11*x9 + x7*x8) + x15*(-x13 - x14*x9) - x3*x4*x6;
226 hx[4] = q[4]*x[4] + x12*x13*x4 + x15*x16*x4 - x6*x8*x9;
227 hx[5] = q[5]*x[5] + x12*(x11 - x16*x9) + x15*(x13*x9 + x14);
228 hx[6] = lmd[0] + (1.0/2.0)*q[6]*(-4*x2 + 2*x[6]);
229 hx[7] = lmd[1] + (1.0/2.0)*q[7]*(-4*x1 + 2*x[7]);
230 hx[8] = lmd[2] + (1.0/2.0)*q[8]*(2*x[8] - 4*cos(t));
231 hx[9] = lmd[3] + q[9]*x[9] + x18*x[11] + x20*x[10];
232 hx[10] = lmd[4] + q[10]*x[10] + x20*x[9] + x21*x[11];
233 hx[11] = -gamma*x19 + lmd[5] + q[11]*x[11] + x18*x[9] + x21*x[10];
234
235 }
236
248 void eval_hu(const double t, const double* x, const double* u,
249 const double* lmd, double* hu) const {
250 const double x0 = (1.0/3.0)*g*m;
251 const double x1 = (1.0/2.0)*sqrt(3)*l*lmd[10]/Iyy;
252 const double x2 = -x1;
253 const double x3 = l*lmd[9]/Ixx;
254 const double x4 = (1.0/2.0)*x3;
255 const double x5 = k*lmd[11]/Izz;
256 const double x6 = 1.0/m;
257 const double x7 = sin(x[3]);
258 const double x8 = sin(x[5]);
259 const double x9 = cos(x[5]);
260 const double x10 = cos(x[3]);
261 const double x11 = sin(x[4]);
262 const double x12 = lmd[6]*x6*(x10*x11*x9 + x7*x8) + lmd[7]*x6*(x10*x11*x8 - x7*x9) + lmd[8]*x10*x6*cos(x[4]);
263 const double x13 = x12 - x5;
264 const double x14 = x13 - x4;
265 const double x15 = x12 + x5;
266 const double x16 = x15 + x4;
267 hu[0] = (1.0/2.0)*r[0]*(2*u[0] - x0) + x14 + x2;
268 hu[1] = (1.0/2.0)*r[1]*(2*u[1] - x0) + x15 - x3;
269 hu[2] = (1.0/2.0)*r[2]*(2*u[2] - x0) + x1 + x14;
270 hu[3] = (1.0/2.0)*r[3]*(2*u[3] - x0) + x1 + x16;
271 hu[4] = (1.0/2.0)*r[4]*(2*u[4] - x0) + x13 + x3;
272 hu[5] = (1.0/2.0)*r[5]*(2*u[5] - x0) + x16 + x2;
273
274 }
275
283 template <typename VectorType1, typename VectorType2, typename VectorType3>
284 void eval_f(const double t, const MatrixBase<VectorType1>& x,
285 const MatrixBase<VectorType2>& u,
286 const MatrixBase<VectorType3>& dx) const {
287 if (x.size() != nx) {
288 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
289 }
290 if (u.size() != nu) {
291 throw std::invalid_argument("[OCP]: u.size() must be " + std::to_string(nu));
292 }
293 if (dx.size() != nx) {
294 throw std::invalid_argument("[OCP]: dx.size() must be " + std::to_string(nx));
295 }
296 eval_f(t, x.derived().data(), u.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType3, dx).data());
297 }
298
306 template <typename VectorType1, typename VectorType2>
307 void eval_phix(const double t, const MatrixBase<VectorType1>& x,
308 const MatrixBase<VectorType2>& phix) const {
309 if (x.size() != nx) {
310 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
311 }
312 if (phix.size() != nx) {
313 throw std::invalid_argument("[OCP]: phix.size() must be " + std::to_string(nx));
314 }
315 eval_phix(t, x.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType2, phix).data());
316 }
317
327 template <typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
328 void eval_hx(const double t, const MatrixBase<VectorType1>& x,
329 const MatrixBase<VectorType2>& uc,
330 const MatrixBase<VectorType3>& lmd,
331 const MatrixBase<VectorType4>& hx) const {
332 if (x.size() != nx) {
333 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
334 }
335 if (uc.size() != nuc) {
336 throw std::invalid_argument("[OCP]: uc.size() must be " + std::to_string(nuc));
337 }
338 if (lmd.size() != nx) {
339 throw std::invalid_argument("[OCP]: lmd.size() must be " + std::to_string(nx));
340 }
341 if (hx.size() != nuc) {
342 throw std::invalid_argument("[OCP]: hx.size() must be " + std::to_string(nx));
343 }
344 eval_hx(t, x.derived().data(), uc.derived().data(), lmd.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType4, hx).data());
345 }
346
356 template <typename VectorType1, typename VectorType2, typename VectorType3, typename VectorType4>
357 void eval_hu(const double t, const MatrixBase<VectorType1>& x,
358 const MatrixBase<VectorType2>& uc,
359 const MatrixBase<VectorType3>& lmd,
360 const MatrixBase<VectorType4>& hu) const {
361 if (x.size() != nx) {
362 throw std::invalid_argument("[OCP]: x.size() must be " + std::to_string(nx));
363 }
364 if (uc.size() != nuc) {
365 throw std::invalid_argument("[OCP]: uc.size() must be " + std::to_string(nuc));
366 }
367 if (lmd.size() != nx) {
368 throw std::invalid_argument("[OCP]: lmd.size() must be " + std::to_string(nx));
369 }
370 if (hu.size() != nuc) {
371 throw std::invalid_argument("[OCP]: hu.size() must be " + std::to_string(nuc));
372 }
373 eval_hu(t, x.derived().data(), uc.derived().data(), lmd.derived().data(), CGMRES_EIGEN_CONST_CAST(VectorType4, hu).data());
374 }
375
376};
377
378} // namespace cgmres
379
380#endif // CGMRES_OCP_HPP_
cgmres::OCP_hexacopter
Definition of the optimal control problem (OCP) of hexacopter.
Definition: ocp.hpp:22
cgmres::OCP_hexacopter::umin
std::array< double, nub > umin
Definition: ocp.hpp:70
cgmres::OCP_hexacopter::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:307
cgmres::OCP_hexacopter::q_terminal
std::array< double, 12 > q_terminal
Definition: ocp.hpp:66
cgmres::OCP_hexacopter::Iyy
double Iyy
Definition: ocp.hpp:59
cgmres::OCP_hexacopter::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:198
cgmres::OCP_hexacopter::nh
static constexpr int nh
Dimension of the Fischer-Burmeister function (already counded in nc).
Definition: ocp.hpp:43
cgmres::OCP_hexacopter::Izz
double Izz
Definition: ocp.hpp:60
cgmres::OCP_hexacopter::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:127
cgmres::OCP_hexacopter::z_ref
double z_ref
Definition: ocp.hpp:63
cgmres::OCP_hexacopter::Ixx
double Ixx
Definition: ocp.hpp:58
cgmres::OCP_hexacopter::disp
void disp(std::ostream &os) const
Definition: ocp.hpp:74
cgmres::OCP_hexacopter::synchronize
void synchronize()
Synchrozies the internal parameters of this OCP with the external references. This method is called a...
Definition: ocp.hpp:115
cgmres::OCP_hexacopter::ubound_indices
static constexpr std::array< int, nub > ubound_indices
Definition: ocp.hpp:69
cgmres::OCP_hexacopter::dummy_weight
std::array< double, nub > dummy_weight
Definition: ocp.hpp:72
cgmres::OCP_hexacopter::umax
std::array< double, nub > umax
Definition: ocp.hpp:71
cgmres::OCP_hexacopter::nub
static constexpr int nub
Dimension of the bound constraints on the control input.
Definition: ocp.hpp:53
cgmres::OCP_hexacopter::nc
static constexpr int nc
Dimension of the equality constraints.
Definition: ocp.hpp:38
cgmres::OCP_hexacopter::k
double k
Definition: ocp.hpp:57
cgmres::OCP_hexacopter::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:248
cgmres::OCP_hexacopter::gamma
double gamma
Definition: ocp.hpp:61
cgmres::OCP_hexacopter::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:328
cgmres::OCP_hexacopter::q
std::array< double, 12 > q
Definition: ocp.hpp:65
cgmres::OCP_hexacopter::g
double g
Definition: ocp.hpp:62
cgmres::OCP_hexacopter::operator<<
friend std::ostream & operator<<(std::ostream &os, const OCP_hexacopter &ocp)
Definition: ocp.hpp:105
cgmres::OCP_hexacopter::l
double l
Definition: ocp.hpp:56
cgmres::OCP_hexacopter::r
std::array< double, 6 > r
Definition: ocp.hpp:67
cgmres::OCP_hexacopter::nuc
static constexpr int nuc
Dimension of the concatenation of the control input and equality constraints.
Definition: ocp.hpp:48
cgmres::OCP_hexacopter::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:357
cgmres::OCP_hexacopter::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:284
cgmres::OCP_hexacopter::nu
static constexpr int nu
Dimension of the control input.
Definition: ocp.hpp:33
cgmres::OCP_hexacopter::nx
static constexpr int nx
Dimension of the state.
Definition: ocp.hpp:28
cgmres::OCP_hexacopter::m
double m
Definition: ocp.hpp:55
cgmres::OCP_hexacopter::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_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