Commit c79c6191 authored by gkharish's avatar gkharish
Browse files

MPC bug fixed. DDP and MPC work in Release mode!

parent a9e54ae4
......@@ -6,8 +6,8 @@ using namespace std;
CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
{
Q <<1e4*1, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e4*1, 0.0, 0.0, 0,0,0,0,
Q <<1e-2*1, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e-2*1, 0.0, 0.0, 0,0,0,0,
2.0e-2*0.0, 0.0, 1e-5*0.0, 0.0, 0,0,0,0,
0.0, 0.0, 0.0, 1e-5*0.0, 0,0,0,0,
0,0,0,0, 0,0,0,0,
......@@ -26,9 +26,9 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0;*/
Qf = Q; //Qf(0,0) = 0; Qf(1,1) = 0;
R << 1e-5,0,
0, 1e-5;
Qf = Q; Qf(0,0) = 0; Qf(1,1) = 0;
R << 1e-6,0,
0, 1e-6;
lxx = Q;
luu = R;
//lux << 0.0,0.0;
......
......@@ -18,15 +18,16 @@ int main()
{
struct timeval tbegin,tend;
double texec=0.0;
double tsec = 0.0; long double tusec = 0.0;
stateVec_t xinit,xDes;
xinit << 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
xDes << 0.5,1.0,0.0,0.0,0.0,0.0,0.0,0.0;
unsigned int T = 40;
double dt=5e-3;
unsigned int T = 400;
double dt=10e-3;
unsigned int iterMax = 100;
double stopCrit = 1e-2;
double stopCrit = 1e-5;
stateVecTab_t xList1;
commandVecTab_t uList1;
ILQRSolver::traj lastTraj;
......@@ -50,7 +51,7 @@ int main()
uList1 = lastTraj.uList;
unsigned int iter = lastTraj.iter;
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
/*texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
texec /= N;
cout << endl;
......@@ -58,12 +59,17 @@ int main()
cout << texec << endl;
cout << "temps d'execution par pas de temps ";
cout << texec/T << endl;
cout << "Nombre d'itérations : " << iter << endl;
cout << "Nombre d'itérations : " << iter << endl;*/
tsec = (double)(tend.tv_sec - tbegin.tv_sec);
tusec = (long double)(tend.tv_usec - tbegin.tv_usec);
if (tusec < 0.0)tsec -=1;
texec = 1e-6*(tsec*1e6 + abs(tusec));
cout << "Time of execution of solver for the entire trrajectory ";
cout << texec<< endl;
cout << "Time of execution of each step of MPC ";
cout << texec/(T) << endl;
cout << "Number of itérations : " << iter << endl;
ofstream fichier("results.csv",ios::out | ios::trunc);
if(fichier)
{
......
......@@ -23,19 +23,20 @@ int main()
cout << endl;
struct timeval tbegin,tend;
double texec=0.0;
double tsec = 0.0; long double tusec = 0.0;
stateVec_t xinit,xDes;
xinit << 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
xDes << 0.5,1.0,0.0,0.0,0.0,0.0,0.0,0.0;
xDes << 0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
//x = xinit;
unsigned int T = 40;
unsigned int M = 100;
double dt=5e-3;
unsigned int iterMax = 30;
double stopCrit = 1e-2;
unsigned int M = 400;
double dt=10e-3;
double preview = 10;
unsigned int iterMax = 100;
double stopCrit = 1e-3;
stateVecTab_t xList;
commandVecTab_t uList;
ILQRSolver::traj lastTraj;
......@@ -45,8 +46,6 @@ int main()
ILQRSolver testSolverRomeoActuator(model,cost,DISABLE_FULLDDP,DISABLE_QPBOX);
ofstream fichier("resultsMPC.csv",ios::out | ios::trunc);
if(!fichier)
{
......@@ -56,42 +55,47 @@ int main()
fichier << T << "," << M << endl;
fichier << "pos1,pos2,vel1,vel2,u1,u2" << endl;
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
gettimeofday(&tbegin,NULL);
for(int i=0;i<M;i++)
{
xDes(0) = 0.25*(preview+i)*dt;
xDes(1) = 0.50*(preview+i)*dt;
testSolverRomeoActuator.initSolver(xinit,xDes);
testSolverRomeoActuator.solveTrajectory();
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
uList = lastTraj.uList;
xinit = xList[1];
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) << "," << uList[j](1,0) << endl;
fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << "," << 0.0 << endl;
fichier << xList[1](0,0) << "," << xList[1](1,0) << "," << xList[1](2,0) << "," << xList[1](3,0) << "," << uList[0](0,0) << "," << uList[0](1,0) << endl;
//fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << "," << 0.0 << endl;
// cout << i;
/*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) << "," << uList[j](1,0) << endl;
fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << "," << 0.0 << endl;*/
}
//fichier << xList[T](0,0) << "," << xList[T](1,0) << "," << xList[T](2,0) << "," << xList[T](3,0) << "," << 0.0 << "," << 0.0 << endl;
gettimeofday(&tend,NULL);
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
/*texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
texec = (double)(tend.tv_usec - tbegin.tv_usec);
cout << "temps d'execution total du solveur ";
cout << texec/1000000.0 << endl;
cout << "temps d'execution par pas de MPC ";
cout << texec/(T*1000000) << endl;
// fichier.close();
cout << texec/(M*1000000) << endl;*/
tsec = (double)(tend.tv_sec - tbegin.tv_sec);
tusec = (long double)(tend.tv_usec - tbegin.tv_usec);
if (tusec < 0.0)tsec -=1;
texec = 1e-6*(tsec*1e6 + abs(tusec));
cout << "Time of execution of solver for the entire trrajectory ";
cout << texec << endl;
cout << "Time of execution of each step of MPC ";
cout << texec/(M) << endl;
// fichier.close();
return 0;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment