Skip to content

Commit fd3e076

Browse files
author
Olivier Stasse
authored
Merge pull request #1 from flforget/mergePR
Merge previous Pull request
2 parents 92075ba + e9f7d42 commit fd3e076

File tree

8 files changed

+499
-487
lines changed

8 files changed

+499
-487
lines changed

‎examples/romeo_actuator/main.cpp‎

Lines changed: 86 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -7,104 +7,101 @@
77
#include "romeosimpleactuator.hh"
88
#include "costfunctionromeoactuator.hh"
99

10-
1110
using namespace std;
1211
using namespace Eigen;
1312

1413
int main()
1514
{
16-
struct timeval tbegin,tend;
17-
double texec=0.0;
18-
DDPSolver<double,4,1>::stateVec_t xinit,xDes,x;
19-
DDPSolver<double,4,1>::commandVec_t u;
20-
21-
xinit << -1.0,0.0,-100.0,0.0;
22-
xDes << 0.5,0.0,0.0,0.0;
23-
24-
int i;
25-
unsigned int T = 3000;
26-
double dt=1e-3;
27-
unsigned int iterMax = 100;
28-
double stopCrit = 1e-5;
29-
DDPSolver<double,4,1>::stateVecTab_t xList;
30-
DDPSolver<double,4,1>::commandVecTab_t uList;
31-
DDPSolver<double,4,1>::traj lastTraj;
32-
33-
RomeoSimpleActuator romeoActuatorModel(dt);
34-
RomeoSimpleActuator* romeoNoisyModel=NULL;
35-
CostFunctionRomeoActuator costRomeoActuator;
36-
DDPSolver<double,4,1> testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,DISABLE_FULLDDP,DISABLE_QPBOX);
37-
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
38-
39-
int N = 100;
40-
gettimeofday(&tbegin,NULL);
41-
testSolverRomeoActuator.solveTrajectory();
42-
gettimeofday(&tend,NULL);
43-
44-
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
45-
xList = lastTraj.xList;
46-
uList = lastTraj.uList;
47-
unsigned int iter = lastTraj.iter;
48-
49-
50-
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
51-
texec /= N;
52-
53-
cout << endl;
54-
cout << "temps d'execution total du solveur ";
55-
cout << texec << endl;
56-
cout << "temps d'execution par pas de temps ";
57-
cout << texec/T << endl;
58-
cout << "Nombre d'itérations : " << iter << endl;
59-
60-
61-
ofstream fichier1("results1.csv",ios::out | ios::trunc);
62-
if(fichier1)
15+
struct timeval tbegin, tend;
16+
double texec = 0.0;
17+
DDPSolver<double, 4, 1>::stateVec_t xinit, xDes, x;
18+
DDPSolver<double, 4, 1>::commandVec_t u;
19+
20+
xinit << -1.0, 0.0, -100.0, 0.0;
21+
xDes << 0.5, 0.0, 0.0, 0.0;
22+
23+
unsigned int T = 3000;
24+
double dt = 1e-3;
25+
unsigned int iterMax = 100;
26+
double stopCrit = 1e-5;
27+
DDPSolver<double, 4, 1>::stateVecTab_t xList;
28+
DDPSolver<double, 4, 1>::commandVecTab_t uList;
29+
DDPSolver<double, 4, 1>::traj lastTraj;
30+
31+
RomeoSimpleActuator romeoActuatorModel(dt);
32+
RomeoSimpleActuator* romeoNoisyModel = NULL;
33+
CostFunctionRomeoActuator costRomeoActuator;
34+
DDPSolver<double, 4, 1> testSolverRomeoActuator(romeoActuatorModel,
35+
costRomeoActuator, DISABLE_FULLDDP, DISABLE_QPBOX);
36+
testSolverRomeoActuator.FirstInitSolver(xinit, xDes, T, dt, iterMax,
37+
stopCrit);
38+
39+
int N = 100;
40+
gettimeofday(&tbegin, NULL);
41+
testSolverRomeoActuator.solveTrajectory();
42+
gettimeofday(&tend, NULL);
43+
44+
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
45+
xList = lastTraj.xList;
46+
uList = lastTraj.uList;
47+
unsigned int iter = lastTraj.iter;
48+
49+
texec = ((double) (1000 * (tend.tv_sec - tbegin.tv_sec)
50+
+ ((tend.tv_usec - tbegin.tv_usec) / 1000))) / 1000.;
51+
texec /= N;
52+
53+
cout << endl;
54+
cout << "temps d'execution total du solveur ";
55+
cout << texec << endl;
56+
cout << "temps d'execution par pas de temps ";
57+
cout << texec / T << endl;
58+
cout << "Nombre d'itérations : " << iter << endl;
59+
60+
ofstream fichier1("results1.csv", ios::out | ios::trunc);
61+
if (fichier1)
62+
{
63+
fichier1 << "tau,tauDot,q,qDot,u" << endl;
64+
x = xinit;
65+
fichier1 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << "," << x(3, 0)
66+
<< "," << uList[0] << endl;
67+
for (unsigned i = 1; i < T; i++)
6368
{
64-
fichier1 << "tau,tauDot,q,qDot,u" << endl;
65-
x = xinit;
66-
fichier1 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
67-
<< x(3, 0) << "," << uList[0] << endl;
68-
for (i = 1; i < T; i++)
69-
{
70-
x = romeoActuatorModel.computeNextState(dt, x, uList[i - 1]);
71-
fichier1 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
72-
<< x(3, 0) << "," << uList[i - 1] << endl;
73-
}
74-
fichier1 << xList[T](0, 0) << "," << xList[T](1, 0) << "," << xList[T](2, 0) << "," << xList[T](3, 0) << ","
75-
<< uList[T - 1](0, 0) << endl;
76-
fichier1.close();
69+
x = romeoActuatorModel.computeNextState(dt, x, uList[i - 1]);
70+
fichier1 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << "," << x(3, 0)
71+
<< "," << uList[i - 1] << endl;
7772
}
78-
else
79-
cerr << "erreur ouverte fichier" << endl;
80-
81-
ofstream fichier2("results2.csv",ios::out | ios::trunc);
82-
if(fichier2)
73+
fichier1 << xList[T](0, 0) << "," << xList[T](1, 0) << "," << xList[T](2, 0)
74+
<< "," << xList[T](3, 0) << "," << uList[T - 1](0, 0) << endl;
75+
fichier1.close();
76+
}
77+
else
78+
cerr << "erreur ouverte fichier" << endl;
79+
80+
ofstream fichier2("results2.csv", ios::out | ios::trunc);
81+
if (fichier2)
82+
{
83+
fichier2 << "tau,tauDot,q,qDot,u" << endl;
84+
fichier2 << T << ',' << 0 << endl;
85+
for (int j = 0; j < 0; j++)
8386
{
84-
fichier2 << "tau,tauDot,q,qDot,u" << endl;
85-
fichier2 << T << ',' << 0 << endl;
86-
for(int j=0;j<0;j++)
87-
{
88-
romeoNoisyModel = new RomeoSimpleActuator(dt,1);
89-
fichier2 << xList[i](0, 0) << "," << xList[i](1, 0) << "," << xList[i](2, 0) << "," << xList[i](3, 0) << ","
90-
<< uList[i](0, 0) << endl;
91-
x = xinit;
92-
for (i = 1; i < T; i++)
93-
{
94-
x = romeoNoisyModel->computeNextState(dt, x, uList[i - 1]);
95-
fichier2 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
96-
<< x(3, 0) << "," << uList[i - 1] << endl;
97-
}
98-
fichier2 << xList[T](0, 0) << "," << xList[T](1, 0) << "," << xList[T](2, 0) << "," << xList[T](3, 0) << ","
99-
<< uList[T - 1](0, 0) << endl;
100-
delete romeoNoisyModel;
101-
}
102-
fichier2.close();
87+
romeoNoisyModel = new RomeoSimpleActuator(dt, 1);
88+
x = xinit;
89+
for (unsigned int i = 1; i < T; i++)
90+
{
91+
x = romeoNoisyModel->computeNextState(dt, x, uList[i - 1]);
92+
fichier2 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
93+
<< x(3, 0) << "," << uList[i - 1] << endl;
94+
}
95+
fichier2 << xList[T](0, 0) << "," << xList[T](1, 0) << ","
96+
<< xList[T](2, 0) << "," << xList[T](3, 0) << ","
97+
<< uList[T - 1](0, 0) << endl;
98+
delete romeoNoisyModel;
10399
}
104-
else
105-
cerr << "erreur ouverte fichier" << endl;
106-
100+
fichier2.close();
101+
}
102+
else
103+
cerr << "erreur ouverte fichier" << endl;
107104

108-
return 0;
105+
return 0;
109106

110107
}

‎examples/romeo_actuator/mainMPC.cpp‎

Lines changed: 68 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -8,85 +8,78 @@
88
#include "romeosimpleactuator.hh"
99
#include "costfunctionromeoactuator.hh"
1010

11-
1211
using namespace std;
1312
using namespace Eigen;
1413

1514
int main()
1615
{
17-
cout << endl;
18-
struct timeval tbegin,tend;
19-
double texec=0.0;
20-
DDPSolver<double,4,1>::stateVec_t xinit,xDes;
21-
22-
xinit << 0.0,0.0,0.0,0.0;
23-
xDes << 1.0,0.0,0.0,0.0;
24-
25-
unsigned int T = 100;
26-
unsigned int M = 3000;
27-
double dt=1e-3;
28-
unsigned int iterMax = 100;
29-
double stopCrit = 1e-3;
30-
DDPSolver<double,4,1>::stateVecTab_t xList;
31-
DDPSolver<double,4,1>::commandVecTab_t uList;
32-
DDPSolver<double,4,1>::traj lastTraj;
33-
34-
srand(time(NULL));
35-
36-
RomeoSimpleActuator romeoActuatorModel(dt);
37-
RomeoSimpleActuator romeoNoisyModel(dt,1);
38-
CostFunctionRomeoActuator costRomeoActuator;
39-
DDPSolver<double,4,1> testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator,DISABLE_FULLDDP,DISABLE_QPBOX);
40-
41-
42-
43-
ofstream fichier("resultsMPC.csv",ios::out | ios::trunc);
44-
if(!fichier)
45-
{
46-
cerr << "erreur fichier ! " << endl;
47-
return 1;
48-
}
49-
fichier << T << "," << M << endl;
50-
fichier << "tau,tauDot,q,qDot,u" << endl;
51-
52-
53-
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
54-
55-
gettimeofday(&tbegin,NULL);
56-
for(int i=0;i<M;i++)
57-
{
58-
testSolverRomeoActuator.initSolver(xinit,xDes);
59-
testSolverRomeoActuator.solveTrajectory();
60-
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
61-
xList = lastTraj.xList;
62-
uList = lastTraj.uList;
63-
xinit = xinit;
64-
xinit = romeoNoisyModel.computeNextState(dt,xinit,uList[0]);
65-
66-
for(int j=0;j<T;j++) fichier << xList[j](0,0) << "," << xList[j](1,0) << "," << xList[j](2,0) << "," << xList[j](3,0) << "," << uList[j](0,0) << endl;
67-
fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << endl;
68-
}
69-
gettimeofday(&tend,NULL);
70-
71-
72-
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
73-
texec = (double)(tend.tv_usec - tbegin.tv_usec);
74-
75-
cout << "temps d'execution total du solveur ";
76-
cout << texec/1000000.0 << endl;
77-
cout << "temps d'execution par pas de MPC ";
78-
cout << texec/(T*1000000) << endl;
79-
80-
fichier.close();
81-
82-
83-
84-
85-
86-
87-
88-
89-
90-
return 0;
16+
cout << endl;
17+
struct timeval tbegin, tend;
18+
double texec = 0.0;
19+
DDPSolver<double, 4, 1>::stateVec_t xinit, xDes;
20+
21+
xinit << 0.0, 0.0, 0.0, 0.0;
22+
xDes << 1.0, 0.0, 0.0, 0.0;
23+
24+
unsigned int T = 100;
25+
unsigned int M = 3000;
26+
double dt = 1e-3;
27+
unsigned int iterMax = 100;
28+
double stopCrit = 1e-3;
29+
DDPSolver<double, 4, 1>::stateVecTab_t xList;
30+
DDPSolver<double, 4, 1>::commandVecTab_t uList;
31+
DDPSolver<double, 4, 1>::traj lastTraj;
32+
33+
srand(static_cast<unsigned int>(time(NULL)));
34+
35+
RomeoSimpleActuator romeoActuatorModel(dt);
36+
RomeoSimpleActuator romeoNoisyModel(dt, 1);
37+
CostFunctionRomeoActuator costRomeoActuator;
38+
DDPSolver<double, 4, 1> testSolverRomeoActuator(romeoActuatorModel,
39+
costRomeoActuator, DISABLE_FULLDDP, DISABLE_QPBOX);
40+
41+
ofstream fichier("resultsMPC.csv", ios::out | ios::trunc);
42+
if (!fichier)
43+
{
44+
cerr << "erreur fichier ! " << endl;
45+
return 1;
46+
}
47+
fichier << T << "," << M << endl;
48+
fichier << "tau,tauDot,q,qDot,u" << endl;
49+
50+
testSolverRomeoActuator.FirstInitSolver(xinit, xDes, T, dt, iterMax,
51+
stopCrit);
52+
53+
gettimeofday(&tbegin, NULL);
54+
for (unsigned int i = 0; i < M; i++)
55+
{
56+
testSolverRomeoActuator.initSolver(xinit, xDes);
57+
testSolverRomeoActuator.solveTrajectory();
58+
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
59+
xList = lastTraj.xList;
60+
uList = lastTraj.uList;
61+
xinit = romeoNoisyModel.computeNextState(dt, xinit, uList[0]);
62+
63+
for (unsigned int j = 0; j < T; j++)
64+
fichier << xList[j](0, 0) << "," << xList[j](1, 0) << ","
65+
<< xList[j](2, 0) << "," << xList[j](3, 0) << "," << uList[j](0, 0)
66+
<< endl;
67+
fichier << xList[T](0, 0) << "," << xList[T](1, 0) << "," << xList[T](2, 0)
68+
<< "," << xList[T](3, 0) << "," << 0.0 << endl;
69+
}
70+
gettimeofday(&tend, NULL);
71+
72+
texec = ((double) (1000 * (tend.tv_sec - tbegin.tv_sec)
73+
+ ((tend.tv_usec - tbegin.tv_usec) / 1000))) / 1000.;
74+
texec = (double) (tend.tv_usec - tbegin.tv_usec);
75+
76+
cout << "temps d'execution total du solveur ";
77+
cout << texec / 1000000.0 << endl;
78+
cout << "temps d'execution par pas de MPC ";
79+
cout << texec / (T * 1000000) << endl;
80+
81+
fichier.close();
82+
83+
return 0;
9184

9285
}

‎examples/romeo_actuator/romeosimpleactuator.cpp‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ RomeoSimpleActuator::RomeoSimpleActuator(double& mydt,bool noiseOnParameters)
3434
else
3535
{
3636
gettimeofday(&tv,NULL);
37-
srand(tv.tv_usec);
37+
srand(static_cast<unsigned int>(tv.tv_usec));
3838
k = 588.0 + 0.0*588.0*0.1*(2.0*(rand()/(double)RAND_MAX)-1.0);
3939
R = 96.1 + 96.1*0.1*(2.0*(rand()/(double)RAND_MAX)-1.0);
4040
Jm = 183*1e-7 + 183*1e-7*0.1*(2.0*(rand()/(double)RAND_MAX)-1.0);
@@ -83,7 +83,9 @@ RomeoSimpleActuator::RomeoSimpleActuator(double& mydt,bool noiseOnParameters)
8383
}
8484

8585

86-
RomeoSimpleActuator::stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
86+
RomeoSimpleActuator::stateVec_t
87+
RomeoSimpleActuator::
88+
computeNextState(double& , const stateVec_t& X,const commandVec_t& U)
8789
{
8890
stateVec_t x_next;//,k1,k2,k3,k4;
8991
/*k1 = A*X + B*U;
@@ -112,17 +114,17 @@ void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,c
112114
}
113115
}
114116

115-
RomeoSimpleActuator::stateMat_t RomeoSimpleActuator::computeTensorContxx(const stateVec_t& nextVx)
117+
RomeoSimpleActuator::stateMat_t RomeoSimpleActuator::computeTensorContxx(const stateVec_t& )
116118
{
117119
return QxxCont;
118120
}
119121

120-
RomeoSimpleActuator::commandMat_t RomeoSimpleActuator::computeTensorContuu(const stateVec_t& nextVx)
122+
RomeoSimpleActuator::commandMat_t RomeoSimpleActuator::computeTensorContuu(const stateVec_t& )
121123
{
122124
return QuuCont;
123125
}
124126

125-
RomeoSimpleActuator::commandR_stateC_t RomeoSimpleActuator::computeTensorContux(const stateVec_t& nextVx)
127+
RomeoSimpleActuator::commandR_stateC_t RomeoSimpleActuator::computeTensorContux(const stateVec_t& )
126128
{
127129
return QuxCont;
128130
}

0 commit comments

Comments
 (0)