1 |
|
|
#include "ddp-actuator-solver/romeo_actuator/romeosimpleactuator.hh" |
2 |
|
|
|
3 |
|
|
#include <math.h> |
4 |
|
|
#include <sys/time.h> |
5 |
|
|
|
6 |
|
|
#include <eigen3/unsupported/Eigen/MatrixFunctions> |
7 |
|
|
|
8 |
|
|
/* |
9 |
|
|
* x0 -> actuator position |
10 |
|
|
* x1 -> actuator speed |
11 |
|
|
* x2 -> motor position |
12 |
|
|
* x3 -> motor speed |
13 |
|
|
*/ |
14 |
|
|
|
15 |
|
|
RomeoSimpleActuator::RomeoSimpleActuator(double& mydt, bool noiseOnParameters) { |
16 |
|
|
stateNb = 4; |
17 |
|
|
commandNb = 1; |
18 |
|
|
dt = mydt; |
19 |
|
|
struct timeval tv; |
20 |
|
|
|
21 |
|
|
if (!noiseOnParameters) { |
22 |
|
|
k = 588.0; |
23 |
|
|
R = 96.1; |
24 |
|
|
Jm = 183 * 1e-7; |
25 |
|
|
Jl = 0.000085; |
26 |
|
|
fvm = 5.65e-5; |
27 |
|
|
fvl = 0.278; |
28 |
|
|
Kt = 0.0578; |
29 |
|
|
mu = 0.52; |
30 |
|
|
Cf0 = 0.0; |
31 |
|
|
a = 0.0; |
32 |
|
|
} else { |
33 |
|
|
gettimeofday(&tv, NULL); |
34 |
|
|
srand(static_cast<unsigned int>(tv.tv_usec)); |
35 |
|
|
k = 588.0 + 0.0 * 588.0 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
36 |
|
|
R = 96.1 + 96.1 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
37 |
|
|
Jm = 183 * 1e-7 + |
38 |
|
|
183 * 1e-7 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
39 |
|
|
Jl = 0.000085 + |
40 |
|
|
0.0 * 0.000085 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
41 |
|
|
fvm = 5.65e-5 + 5.65e-5 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
42 |
|
|
fvl = 0.278 + 0.0 * 0.278 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
43 |
|
|
Kt = 0.0578 + 0.0578 * 0.1 * (2.0 * (rand() / (double)RAND_MAX) - 1.0); |
44 |
|
|
mu = 0.52; |
45 |
|
|
Cf0 = 0.0; |
46 |
|
|
a = 0.0; |
47 |
|
|
} |
48 |
|
|
|
49 |
|
|
Id.setIdentity(); |
50 |
|
|
|
51 |
|
|
A << 0.0, 1.0, 0.0, 0.0, -k / Jl, -fvl / Jl, k / (R * Jl), 0.0, 0.0, 0.0, 0.0, |
52 |
|
|
1.0, k / (R * Jm), 0.0, -k / (Jm * R * R), -fvm / Jm; |
53 |
|
|
|
54 |
|
|
Ad = (dt * A).exp(); |
55 |
|
|
|
56 |
|
|
B << 0.0, 0.0, 0.0, Kt / Jm; |
57 |
|
|
Bd = dt * B; |
58 |
|
|
|
59 |
|
|
fu << 0.0, 0.0, 0.0, Kt / Jm; |
60 |
|
|
fx.setZero(); |
61 |
|
|
|
62 |
|
|
fxx[0].setZero(); |
63 |
|
|
fxx[1].setZero(); |
64 |
|
|
fxx[2].setZero(); |
65 |
|
|
fxx[3].setZero(); |
66 |
|
|
|
67 |
|
|
fxu[0].setZero(); |
68 |
|
|
fxu[0].setZero(); |
69 |
|
|
|
70 |
|
|
fuu[0].setZero(); |
71 |
|
|
fux[0].setZero(); |
72 |
|
|
fxu[0].setZero(); |
73 |
|
|
|
74 |
|
|
QxxCont.setZero(); |
75 |
|
|
QuuCont.setZero(); |
76 |
|
|
QuxCont.setZero(); |
77 |
|
|
|
78 |
|
|
lowerCommandBounds << -1.0; |
79 |
|
|
upperCommandBounds << 1.0; |
80 |
|
|
} |
81 |
|
|
|
82 |
|
|
RomeoSimpleActuator::stateVec_t RomeoSimpleActuator::computeNextState( |
83 |
|
|
double&, const stateVec_t& X, const commandVec_t& U) { |
84 |
|
|
stateVec_t x_next; //,k1,k2,k3,k4; |
85 |
|
|
/*k1 = A*X + B*U; |
86 |
|
|
k2 = A*(X+(dt/2)*k1) + B*U; |
87 |
|
|
k3 = A*(X+(dt/2)*k2) + B*U; |
88 |
|
|
k4 = A*(X+dt*k3) + B*U; |
89 |
|
|
x_next = X + (dt/6)*(k1+2*k2+2*k3+k4);*/ |
90 |
|
|
|
91 |
|
|
x_next = Ad * X + Bd * U; |
92 |
|
|
return x_next; |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
void RomeoSimpleActuator::computeModelDeriv(double& dt, const stateVec_t& X, |
96 |
|
|
const commandVec_t& U) { |
97 |
|
|
double dh = 1e-7; |
98 |
|
|
stateVec_t Xp, Xm; |
99 |
|
|
Xp = X; |
100 |
|
|
Xm = X; |
101 |
|
|
for (int i = 0; i < 4; i++) { |
102 |
|
|
Xp[i] += dh / 2; |
103 |
|
|
Xm[i] -= dh / 2; |
104 |
|
|
fx.col(i) = |
105 |
|
|
(computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U)) / dh; |
106 |
|
|
Xp = X; |
107 |
|
|
Xm = X; |
108 |
|
|
} |
109 |
|
|
} |
110 |
|
|
|
111 |
|
|
RomeoSimpleActuator::stateMat_t RomeoSimpleActuator::computeTensorContxx( |
112 |
|
|
const stateVec_t&) { |
113 |
|
|
return QxxCont; |
114 |
|
|
} |
115 |
|
|
|
116 |
|
|
RomeoSimpleActuator::commandMat_t RomeoSimpleActuator::computeTensorContuu( |
117 |
|
|
const stateVec_t&) { |
118 |
|
|
return QuuCont; |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
RomeoSimpleActuator::commandR_stateC_t RomeoSimpleActuator::computeTensorContux( |
122 |
|
|
const stateVec_t&) { |
123 |
|
|
return QuxCont; |
124 |
|
|
} |