1 |
|
|
#include "ddp-actuator-solver/romeo_actuator/romeotorqueactuator.hh" |
2 |
|
|
|
3 |
|
|
#include <math.h> |
4 |
|
|
#define pi M_PI |
5 |
|
|
|
6 |
|
|
const double RomeoTorqueActuator::k = 588.0; |
7 |
|
|
const double RomeoTorqueActuator::R = 96.1; |
8 |
|
|
const double RomeoTorqueActuator::Jm = 183 * 1e-7; |
9 |
|
|
const double RomeoTorqueActuator::Jl = 0.000085; |
10 |
|
|
const double RomeoTorqueActuator::fvm = 5.65e-5; |
11 |
|
|
const double RomeoTorqueActuator::fvl = 0.278; |
12 |
|
|
const double RomeoTorqueActuator::Kt = 0.0578; |
13 |
|
|
const double RomeoTorqueActuator::mu = 0.52; |
14 |
|
|
const double RomeoTorqueActuator::Cf0 = 0.0; |
15 |
|
|
const double RomeoTorqueActuator::a = 0.0; |
16 |
|
|
|
17 |
|
|
/* |
18 |
|
|
* x0 -> spring torque |
19 |
|
|
* x1 -> actuator position |
20 |
|
|
* x2 -> actuator speed |
21 |
|
|
* x3 -> motor speed |
22 |
|
|
*/ |
23 |
|
|
|
24 |
|
|
RomeoTorqueActuator::RomeoTorqueActuator(double& mydt) { |
25 |
|
|
stateNb = 4; |
26 |
|
|
commandNb = 1; |
27 |
|
|
dt = mydt; |
28 |
|
|
|
29 |
|
|
Id.setIdentity(); |
30 |
|
|
|
31 |
|
|
A << 0.0, 0.0, k, -k / R, 0.0, 0.0, 1.0, 0.0, -1.0 / Jl, 0.0, -fvl / Jl, 0.0, |
32 |
|
|
1 / (R * Jm), 0.0, 0.0, -fvm / Jm; |
33 |
|
|
B << 0.0, 0.0, 0.0, Kt / Jm; |
34 |
|
|
|
35 |
|
|
fu << 0.0, 0.0, 0.0, Kt / Jm; |
36 |
|
|
fx.setZero(); |
37 |
|
|
|
38 |
|
|
fxx[0].setZero(); |
39 |
|
|
fxx[1].setZero(); |
40 |
|
|
fxx[2].setZero(); |
41 |
|
|
fxx[3].setZero(); |
42 |
|
|
|
43 |
|
|
fxu[0].setZero(); |
44 |
|
|
fxu[0].setZero(); |
45 |
|
|
|
46 |
|
|
fuu[0].setZero(); |
47 |
|
|
fux[0].setZero(); |
48 |
|
|
fxu[0].setZero(); |
49 |
|
|
|
50 |
|
|
QxxCont.setZero(); |
51 |
|
|
QuuCont.setZero(); |
52 |
|
|
QuxCont.setZero(); |
53 |
|
|
|
54 |
|
|
lowerCommandBounds << -1.0; |
55 |
|
|
upperCommandBounds << 1.0; |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
RomeoTorqueActuator::stateVec_t RomeoTorqueActuator::computeNextState( |
59 |
|
|
double& dt, const stateVec_t& X, const commandVec_t& U) { |
60 |
|
|
stateVec_t x_next, k1, k2, k3, k4; |
61 |
|
|
k1 = A * X + B * U; |
62 |
|
|
k2 = A * (X + (dt / 2) * k1) + B * U; |
63 |
|
|
k3 = A * (X + (dt / 2) * k2) + B * U; |
64 |
|
|
k4 = A * (X + dt * k3) + B * U; |
65 |
|
|
x_next = X + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4); |
66 |
|
|
|
67 |
|
|
return x_next; |
68 |
|
|
} |
69 |
|
|
|
70 |
|
|
void RomeoTorqueActuator::computeModelDeriv(double& dt, const stateVec_t& X, |
71 |
|
|
const commandVec_t& U) { |
72 |
|
|
double dh = 1e-7; |
73 |
|
|
stateVec_t Xp, Xm; |
74 |
|
|
Xp = X; |
75 |
|
|
Xm = X; |
76 |
|
|
for (int i = 0; i < 4; i++) { |
77 |
|
|
Xp[i] += dh / 2; |
78 |
|
|
Xm[i] -= dh / 2; |
79 |
|
|
fx.col(i) = |
80 |
|
|
(computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U)) / dh; |
81 |
|
|
Xp = X; |
82 |
|
|
Xm = X; |
83 |
|
|
} |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
RomeoTorqueActuator::stateMat_t RomeoTorqueActuator::computeTensorContxx( |
87 |
|
|
const stateVec_t&) { |
88 |
|
|
return QxxCont; |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
RomeoTorqueActuator::commandMat_t RomeoTorqueActuator::computeTensorContuu( |
92 |
|
|
const stateVec_t&) { |
93 |
|
|
return QuuCont; |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
RomeoTorqueActuator::commandR_stateC_t RomeoTorqueActuator::computeTensorContux( |
97 |
|
|
const stateVec_t&) { |
98 |
|
|
return QuxCont; |
99 |
|
|
} |