1 |
|
|
#include "ddp-actuator-solver/linear/modelLinear.hh" |
2 |
|
|
|
3 |
|
|
#include <math.h> |
4 |
|
|
#include <sys/time.h> |
5 |
|
|
|
6 |
|
|
#include <eigen3/unsupported/Eigen/MatrixFunctions> |
7 |
|
|
#include <iostream> |
8 |
|
|
|
9 |
|
|
/* |
10 |
|
|
* x0 -> pendulum angle |
11 |
|
|
* x1 -> pendulum angular velocity |
12 |
|
|
* x2 -> kart position |
13 |
|
|
* x3 -> kart veolicity |
14 |
|
|
*/ |
15 |
|
|
|
16 |
|
|
ModelLinear::ModelLinear(double& mydt, bool) { |
17 |
|
|
stateNb = 2; |
18 |
|
|
commandNb = 1; |
19 |
|
|
dt = mydt; |
20 |
|
|
|
21 |
|
|
Id.setIdentity(); |
22 |
|
|
|
23 |
|
|
fu.setZero(); |
24 |
|
|
fx.setZero(); |
25 |
|
|
|
26 |
|
|
fu << 0.0, dt; |
27 |
|
|
fx << 1.0, dt, 0.0, 1.0; |
28 |
|
|
|
29 |
|
|
fxx[0].setZero(); |
30 |
|
|
fxx[1].setZero(); |
31 |
|
|
|
32 |
|
|
fxu[0].setZero(); |
33 |
|
|
fxu[0].setZero(); |
34 |
|
|
|
35 |
|
|
fuu[0].setZero(); |
36 |
|
|
fux[0].setZero(); |
37 |
|
|
fxu[0].setZero(); |
38 |
|
|
|
39 |
|
|
QxxCont.setZero(); |
40 |
|
|
QuuCont.setZero(); |
41 |
|
|
QuxCont.setZero(); |
42 |
|
|
|
43 |
|
|
lowerCommandBounds << -1.0; |
44 |
|
|
upperCommandBounds << 1.0; |
45 |
|
|
} |
46 |
|
|
|
47 |
|
|
ModelLinear::stateVec_t ModelLinear::computeDeriv(double&, const stateVec_t& X, |
48 |
|
|
const commandVec_t& U) { |
49 |
|
|
dX[0] = X[1]; |
50 |
|
|
dX[1] = U[0]; |
51 |
|
|
return dX; |
52 |
|
|
} |
53 |
|
|
|
54 |
|
|
ModelLinear::stateVec_t ModelLinear::computeNextState(double&, |
55 |
|
|
const stateVec_t& X, |
56 |
|
|
const commandVec_t& U) { |
57 |
|
|
/*k1 = computeDeriv(dt, X, U); |
58 |
|
|
k2 = computeDeriv(dt, X + (dt / 2) * k1, U); |
59 |
|
|
k3 = computeDeriv(dt, X + (dt / 2) * k2, U); |
60 |
|
|
k4 = computeDeriv(dt, X + dt * k3, U); |
61 |
|
|
x_next = X + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4);*/ |
62 |
|
|
x_next = fx * X + fu * U; |
63 |
|
|
return x_next; |
64 |
|
|
} |
65 |
|
|
|
66 |
|
|
void ModelLinear::computeModelDeriv(double&, const stateVec_t&, |
67 |
|
|
const commandVec_t&) { |
68 |
|
|
/*double dh = 1e-7; |
69 |
|
|
stateVec_t Xp, Xm; |
70 |
|
|
commandVec_t Up, Um; |
71 |
|
|
Xp = X; |
72 |
|
|
Xm = X; |
73 |
|
|
Up = U; |
74 |
|
|
Um = U; |
75 |
|
|
for (unsigned int i = 0; i < stateNb; i++) |
76 |
|
|
{ |
77 |
|
|
Xp[i] += dh / 2; |
78 |
|
|
Xm[i] -= dh / 2; |
79 |
|
|
fx.col(i) = (computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U)) |
80 |
|
|
/ dh; |
81 |
|
|
Xp = X; |
82 |
|
|
Xm = X; |
83 |
|
|
} |
84 |
|
|
for (unsigned int i = 0; i < commandNb; i++) |
85 |
|
|
{ |
86 |
|
|
Up[i] += dh / 2; |
87 |
|
|
Um[i] -= dh / 2; |
88 |
|
|
fu.col(i) = (computeNextState(dt, X, Up) - computeNextState(dt, X, Um)) |
89 |
|
|
/ dh; |
90 |
|
|
Up = U; |
91 |
|
|
Um = U; |
92 |
|
|
}*/ |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
ModelLinear::stateMat_t ModelLinear::computeTensorContxx(const stateVec_t&) { |
96 |
|
|
return QxxCont; |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
ModelLinear::commandMat_t ModelLinear::computeTensorContuu(const stateVec_t&) { |
100 |
|
|
return QuuCont; |
101 |
|
|
} |
102 |
|
|
|
103 |
|
|
ModelLinear::commandR_stateC_t ModelLinear::computeTensorContux( |
104 |
|
|
const stateVec_t&) { |
105 |
|
|
return QuxCont; |
106 |
|
|
} |