1 |
|
|
#include <sys/time.h> |
2 |
|
|
#include <time.h> |
3 |
|
|
|
4 |
|
|
#include <ddp-actuator-solver/ddpsolver.hh> |
5 |
|
|
#include <fstream> |
6 |
|
|
#include <iostream> |
7 |
|
|
|
8 |
|
|
#include "ddp-actuator-solver/romeo_actuator/costfunctionromeoactuator.hh" |
9 |
|
|
#include "ddp-actuator-solver/romeo_actuator/romeosimpleactuator.hh" |
10 |
|
|
|
11 |
|
|
using namespace std; |
12 |
|
|
using namespace Eigen; |
13 |
|
|
|
14 |
|
|
int main() { |
15 |
|
|
DDPSolver<double, 4, 1>::stateVec_t xinit, xDes, x; |
16 |
|
|
DDPSolver<double, 4, 1>::commandVec_t u; |
17 |
|
|
|
18 |
|
|
u << 1.0; |
19 |
|
|
|
20 |
|
|
xinit << -1.0, 0.0, -100.0, 0.0; |
21 |
|
|
xDes << 1.0, 0.0, 0.0, 0.0; |
22 |
|
|
|
23 |
|
|
x = xinit; |
24 |
|
|
|
25 |
|
|
double dt = 1e-3; |
26 |
|
|
unsigned int N = 3000; |
27 |
|
|
DDPSolver<double, 4, 1>::stateVecTab_t xList; |
28 |
|
|
DDPSolver<double, 4, 1>::commandVecTab_t uList; |
29 |
|
|
|
30 |
|
|
xList.resize(N); |
31 |
|
|
uList.resize(N); |
32 |
|
|
|
33 |
|
|
RomeoSimpleActuator romeoActuatorModel(dt); |
34 |
|
|
|
35 |
|
|
#ifdef TEST_KP_KI |
36 |
|
|
double last_err = 0.0; |
37 |
|
|
// double err = 0.0; |
38 |
|
|
// double int_err = 0.0; |
39 |
|
|
#endif |
40 |
|
|
for (unsigned int i = 0; i < N; i++) { |
41 |
|
|
#ifdef TEST_KP_KI |
42 |
|
|
/*last_err = err; |
43 |
|
|
err = x[0] - xDes[0]; |
44 |
|
|
int_err += err; |
45 |
|
|
if(int_err>300)int_err=300; |
46 |
|
|
if(int_err<-300)int_err=-300; |
47 |
|
|
u << Kp*err + Ki*int_err;*/ |
48 |
|
|
// u << 1.0*(1.0-x[0]); |
49 |
|
|
// if(u[0]>10.0)u<<10.0; |
50 |
|
|
#endif |
51 |
|
|
xList[i] = x; |
52 |
|
|
uList[i] = u; |
53 |
|
|
x = romeoActuatorModel.computeNextState(dt, x, u); |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
ofstream fichier("results.csv", ios::out | ios::trunc); |
57 |
|
|
if (fichier) { |
58 |
|
|
fichier << "tau,tauDot,q,qDot,u" << endl; |
59 |
|
|
for (unsigned int i = 0; i < N; i++) |
60 |
|
|
fichier << xList[i](0, 0) << "," << xList[i](1, 0) << "," |
61 |
|
|
<< xList[i](2, 0) << "," << xList[i](3, 0) << "," |
62 |
|
|
<< uList[i](0, 0) << endl; |
63 |
|
|
fichier.close(); |
64 |
|
|
} else |
65 |
|
|
cerr << "erreur ouverte fichier" << endl; |
66 |
|
|
return 0; |
67 |
|
|
} |