1 |
|
|
#include "ddp-actuator-solver/pyrene_actuator/pyreneActuator.hh" |
2 |
|
|
|
3 |
|
|
#include <math.h> |
4 |
|
|
|
5 |
|
|
#include <cmath> |
6 |
|
|
#include <iostream> |
7 |
|
|
#define pi M_PI |
8 |
|
|
|
9 |
|
|
const double pyreneActuator::K = 10.6; |
10 |
|
|
const double pyreneActuator::J_j = 0.5878; |
11 |
|
|
const double pyreneActuator::F_vj = 0.4757; |
12 |
|
|
const double pyreneActuator::F_sj = 0.5403; |
13 |
|
|
const double pyreneActuator::offset_j = -0.3012; |
14 |
|
|
const double pyreneActuator::J_m = 0.21; |
15 |
|
|
const double pyreneActuator::F_vm = 5.6714; |
16 |
|
|
const double pyreneActuator::F_sm = 4.0420; |
17 |
|
|
const double pyreneActuator::offset_m = 0.5572; |
18 |
|
|
const double pyreneActuator::M = 4.5667; |
19 |
|
|
const double pyreneActuator::c_y = 1.1005 / M; |
20 |
|
|
const double pyreneActuator::c_x = -0.0981 / M; |
21 |
|
|
const double pyreneActuator::mu = 1000.0; |
22 |
|
|
const double pyreneActuator::g = 9.81; |
23 |
|
|
|
24 |
|
|
/* |
25 |
|
|
* x0 -> actuator position |
26 |
|
|
* x1 -> actuator speed |
27 |
|
|
*/ |
28 |
|
|
|
29 |
|
|
pyreneActuator::pyreneActuator() { |
30 |
|
|
stateNb = 2; |
31 |
|
|
commandNb = 1; |
32 |
|
|
J = J_m + J_j; |
33 |
|
|
F_v = F_vm + F_vj; |
34 |
|
|
F_s = F_sm + F_sj; |
35 |
|
|
|
36 |
|
|
L = 0.0; |
37 |
|
|
l_y = c_y; |
38 |
|
|
l_x = c_x; |
39 |
|
|
|
40 |
|
|
fu << 0.0, K / J; |
41 |
|
|
fx.setZero(); |
42 |
|
|
|
43 |
|
|
fxx[0].setZero(); |
44 |
|
|
fxx[1].setZero(); |
45 |
|
|
|
46 |
|
|
fxu[0].setZero(); |
47 |
|
|
fux[0].setZero(); |
48 |
|
|
|
49 |
|
|
fuu[0].setZero(); |
50 |
|
|
|
51 |
|
|
QxxCont.setZero(); |
52 |
|
|
QuuCont.setZero(); |
53 |
|
|
QuxCont.setZero(); |
54 |
|
|
|
55 |
|
|
lowerCommandBounds << -1.0; |
56 |
|
|
upperCommandBounds << 1.0; |
57 |
|
|
} |
58 |
|
|
|
59 |
|
|
void pyreneActuator::setLoadParam(const double& mass, const double& coordX, |
60 |
|
|
const double& coordY) { |
61 |
|
|
L = mass; |
62 |
|
|
l_y = coordY; |
63 |
|
|
l_x = coordX; |
64 |
|
|
} |
65 |
|
|
|
66 |
|
|
void pyreneActuator::setLoadMass(const double& mass) { |
67 |
|
|
L = mass; |
68 |
|
|
std::cout << " L : " << L << std::endl; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
void pyreneActuator::removeLoad() { L = 0.0; } |
72 |
|
|
|
73 |
|
|
pyreneActuator::stateVec_t pyreneActuator::computeStateDeriv( |
74 |
|
|
double&, const stateVec_t& X, const commandVec_t& U) { |
75 |
|
|
stateVec_t dX; |
76 |
|
|
|
77 |
|
|
dX[0] = X[1]; |
78 |
|
|
dX[1] = (1 / J) * (K * U[0] - F_v * X[1] - F_s * tanh(mu * X[1])) - |
79 |
|
|
(M * g / J) * (cos(X[0]) * c_x + sin(X[0]) * c_y) - |
80 |
|
|
(L * g / J) * (cos(X[0]) * l_x + sin(X[0]) * l_y) - |
81 |
|
|
(offset_m + offset_j); |
82 |
|
|
return dX; |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
pyreneActuator::stateVec_t pyreneActuator::computeNextState( |
86 |
|
|
double& dt, const stateVec_t& X, const commandVec_t& U) { |
87 |
|
|
stateVec_t x_next, k1, k2, k3, k4; |
88 |
|
|
k1 = computeStateDeriv(dt, X, U); |
89 |
|
|
k2 = computeStateDeriv(dt, X + (dt / 2) * k1, U); |
90 |
|
|
k3 = computeStateDeriv(dt, X + (dt / 2) * k2, U); |
91 |
|
|
k4 = computeStateDeriv(dt, X + dt * k3, U); |
92 |
|
|
x_next = X + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4); |
93 |
|
|
return x_next; |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
void pyreneActuator::computeModelDeriv(double& dt, const stateVec_t& X, |
97 |
|
|
const commandVec_t& U) { |
98 |
|
|
double dh = 1e-7; |
99 |
|
|
stateVec_t Xp, Xm; |
100 |
|
|
Xp = X; |
101 |
|
|
Xm = X; |
102 |
|
|
for (unsigned int i = 0; i < stateNb; i++) { |
103 |
|
|
Xp[i] += dh / 2; |
104 |
|
|
Xm[i] -= dh / 2; |
105 |
|
|
fx.col(i) = |
106 |
|
|
(computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U)) / dh; |
107 |
|
|
Xp = X; |
108 |
|
|
Xm = X; |
109 |
|
|
} |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
pyreneActuator::stateMat_t pyreneActuator::computeTensorContxx( |
113 |
|
|
const stateVec_t&) { |
114 |
|
|
return QxxCont; |
115 |
|
|
} |
116 |
|
|
|
117 |
|
|
pyreneActuator::commandMat_t pyreneActuator::computeTensorContuu( |
118 |
|
|
const stateVec_t&) { |
119 |
|
|
return QuuCont; |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
pyreneActuator::commandR_stateC_t pyreneActuator::computeTensorContux( |
123 |
|
|
const stateVec_t&) { |
124 |
|
|
return QuxCont; |
125 |
|
|
} |