1 |
|
|
#include <sys/time.h> |
2 |
|
|
#include <time.h> |
3 |
|
|
|
4 |
|
|
#include <ddp-actuator-solver/ddpsolver.hh> |
5 |
|
|
#include <fstream> |
6 |
|
|
#include <iostream> |
7 |
|
|
#include <string> |
8 |
|
|
#include <vector> |
9 |
|
|
|
10 |
|
|
#include "ddp-actuator-solver/pyrene_actuator/pyreneActuator.hh" |
11 |
|
|
#include "ddp-actuator-solver/pyrene_actuator/pyreneCostFunction.hh" |
12 |
|
|
|
13 |
|
|
using namespace std; |
14 |
|
|
using namespace Eigen; |
15 |
|
|
|
16 |
|
|
vector<double> fillVector(string repoBags, string fileName) { |
17 |
|
|
vector<double> fillVector(2000); |
18 |
|
|
ifstream file((repoBags + fileName).c_str(), ios::in); |
19 |
|
|
if (!file) { |
20 |
|
|
cerr << "File " << fileName.c_str() << " not Read" << endl; |
21 |
|
|
} |
22 |
|
|
|
23 |
|
|
double val; |
24 |
|
|
for (int j = 0; j < 2000; j++) { |
25 |
|
|
string line; |
26 |
|
|
getline(file, line); |
27 |
|
|
stringstream ss(line); |
28 |
|
|
if (ss >> val) { |
29 |
|
|
fillVector[j] = val; |
30 |
|
|
} |
31 |
|
|
} |
32 |
|
|
return fillVector; |
33 |
|
|
} |
34 |
|
|
|
35 |
|
|
int main(int argc, char *argv[]) { |
36 |
|
|
if (argc != 2) { |
37 |
|
|
cerr << "Convention: mainBags nameRepoBags " << endl; |
38 |
|
|
return 1; |
39 |
|
|
} |
40 |
|
|
string repoBags = argv[1]; |
41 |
|
|
|
42 |
|
|
vector<double> vec_joint_pos = fillVector(repoBags, "joint_pos.txt"); |
43 |
|
|
|
44 |
|
|
struct timeval tbegin, tend; |
45 |
|
|
double texec = 0.0; |
46 |
|
|
DDPSolver<double, 2, 1>::stateVec_t xinit, xDes, x; |
47 |
|
|
DDPSolver<double, 2, 1>::commandVec_t u; |
48 |
|
|
|
49 |
|
|
unsigned int T = 50; |
50 |
|
|
double dt = 1e-3; |
51 |
|
|
unsigned int iterMax = 10; |
52 |
|
|
double stopCrit = 1e-3; // 0.01; |
53 |
|
|
DDPSolver<double, 2, 1>::stateVecTab_t xList; |
54 |
|
|
DDPSolver<double, 2, 1>::commandVecTab_t uList; |
55 |
|
|
DDPSolver<double, 2, 1>::traj lastTraj; |
56 |
|
|
|
57 |
|
|
pyreneActuator pyreneActuator; |
58 |
|
|
CostFunctionPyreneActuator costFunction; |
59 |
|
|
costFunction.setTauLimit(70); |
60 |
|
|
costFunction.setJointLimit(0.0, -2.35619449019); |
61 |
|
|
costFunction.setJointVelLimit(30.0, -30.0); |
62 |
|
|
// CostFunction<double,2,1>::stateMat_t Q; |
63 |
|
|
// Q << 500.0,0.0,0.0,0.01; |
64 |
|
|
// CostFunction<double,2,1>::commandMat_t P; |
65 |
|
|
// P << 100.0; |
66 |
|
|
// costFunction.setCostGainState(Q); |
67 |
|
|
// costFunction.setCostGainTorqueConstraint(P); |
68 |
|
|
// pyreneActuator.setLoadParam(30.0,-0.021481595, 0.10); |
69 |
|
|
DDPSolver<double, 2, 1> testSolverActuator(pyreneActuator, costFunction, |
70 |
|
|
DISABLE_FULLDDP, DISABLE_QPBOX); |
71 |
|
|
|
72 |
|
|
double dx_joint; |
73 |
|
|
dx_joint = 0.5422; |
74 |
|
|
xinit << vec_joint_pos[0], dx_joint; |
75 |
|
|
xDes << vec_joint_pos[1], 0.0; |
76 |
|
|
unsigned int nbIterTestMax = 2000.0; |
77 |
|
|
unsigned int iter; |
78 |
|
|
testSolverActuator.FirstInitSolver(xinit, xDes, T, dt, iterMax, stopCrit); |
79 |
|
|
|
80 |
|
|
for (unsigned int i = 0; i < nbIterTestMax - 1; i++) { |
81 |
|
|
gettimeofday(&tbegin, NULL); |
82 |
|
|
|
83 |
|
|
testSolverActuator.initSolver(xinit, xDes); |
84 |
|
|
testSolverActuator.solveTrajectory(); |
85 |
|
|
lastTraj = testSolverActuator.getLastSolvedTrajectory(); |
86 |
|
|
gettimeofday(&tend, NULL); |
87 |
|
|
xList = lastTraj.xList; |
88 |
|
|
uList = lastTraj.uList; |
89 |
|
|
iter = lastTraj.iter; |
90 |
|
|
|
91 |
|
|
xinit << xList[1](0, 0), xList[1](1, 0); |
92 |
|
|
xDes << vec_joint_pos[i + 1], 0.0; |
93 |
|
|
|
94 |
|
|
texec += ((double)(tend.tv_sec - tbegin.tv_sec) * 1000.0 + |
95 |
|
|
((double)(tend.tv_usec - tbegin.tv_usec) / 1000.0)); |
96 |
|
|
cout << "texec:" << texec << std::endl; |
97 |
|
|
|
98 |
|
|
string ResultNumber; |
99 |
|
|
ostringstream convert; |
100 |
|
|
convert << i; |
101 |
|
|
ResultNumber = convert.str(); |
102 |
|
|
|
103 |
|
|
ofstream file(("results_sinu/results_sinu" + ResultNumber + ".csv").c_str(), |
104 |
|
|
ios::out | ios::trunc); |
105 |
|
|
if (file) { |
106 |
|
|
file << "q,qdes,qDot,qDotDes,u" << endl; |
107 |
|
|
for (unsigned int i = 0; i < T; i++) { |
108 |
|
|
file << xList[i](0, 0) << "," << xDes[0] << "," << xList[i](1, 0) << "," |
109 |
|
|
<< xDes[1] << "," << uList[i - 1](0, 0) << endl; |
110 |
|
|
} |
111 |
|
|
file << xList[T](0, 0) << "," << xDes[0] << "," << xList[T](1, 0) << "," |
112 |
|
|
<< xDes[1] << "," << uList[T - 1](0, 0) << endl; |
113 |
|
|
file.close(); |
114 |
|
|
} else { |
115 |
|
|
cerr << "erreur ouverte fichier" << endl; |
116 |
|
|
} |
117 |
|
|
|
118 |
|
|
if (i == 0) { |
119 |
|
|
ofstream fichier(("results_sinu/results_sinu.csv"), |
120 |
|
|
ios::out | ios::trunc); |
121 |
|
|
if (fichier) { |
122 |
|
|
fichier << "q,qdes,qDot,qDotDes,u" << endl; |
123 |
|
|
} |
124 |
|
|
} |
125 |
|
|
|
126 |
|
|
ofstream fichier(("results_sinu/results_sinu.csv"), ios::out | ios::app); |
127 |
|
|
if (fichier) { |
128 |
|
|
fichier << xList[1](0, 0) << "," << xDes[0] << "," << xList[1](1, 0) |
129 |
|
|
<< "," << xDes[1] << "," << uList[0](0, 0) << endl; |
130 |
|
|
fichier.close(); |
131 |
|
|
} else { |
132 |
|
|
cerr << "erreur ouverte fichier" << endl; |
133 |
|
|
} |
134 |
|
|
} |
135 |
|
|
cout << endl; |
136 |
|
|
cout << "temps d'execution total du solveur "; |
137 |
|
|
cout << texec << endl; |
138 |
|
|
cout << "temps d'execution par pas de temps "; |
139 |
|
|
cout << texec / (double)nbIterTestMax << endl; |
140 |
|
|
cout << "Nombre d'itérations : " << iter << endl; |
141 |
|
|
|
142 |
|
|
return 0; |
143 |
|
|
} |