Commit e6bbaa6c authored by flforget's avatar flforget
Browse files

WOP

parent 82773959
......@@ -49,8 +49,6 @@ void ILQRSolver::solveTrajectory()
{
backwardLoop();
forwardLoop();
xList = updatedxList;
uList = updateduList;
if(changeAmount<stopCrit)
break;
for(unsigned int i=0;i<T;i++)
......@@ -126,7 +124,7 @@ void ILQRSolver::forwardLoop()
{
changeAmount = 0.0;
updatedxList[0] = xInit;
alpha = 0.0;
alpha = 1.0;
for(unsigned int i=0;i<T;i++)
{
updateduList[i] = uList[i] + alpha*kList[i] + KList[i]*(updatedxList[i]-xList[i]);
......@@ -135,3 +133,11 @@ void ILQRSolver::forwardLoop()
changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
}
}
ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
{
lastTraj.xList = updatedxList;
lastTraj.uList = updateduList;
return lastTraj;
}
......@@ -11,6 +11,14 @@ using namespace Eigen;
class ILQRSolver
{
public:
struct traj
{
stateVec_t* xList;
commandVec_t* uList;
};
public:
ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunction);
private:
......@@ -37,6 +45,7 @@ private:
commandVec_t* uList;
stateVec_t* updatedxList;
commandVec_t* updateduList;
struct traj lastTraj;
stateVec_t nextVx;
stateMat_t nextVxx;
......@@ -68,6 +77,7 @@ public:
void initTrajectory();
void backwardLoop();
void forwardLoop();
struct traj getLastSolvedTrajectory();
private:
protected:
......
......@@ -19,13 +19,16 @@ int main()
double texec=0.0;
stateVec_t xinit,xDes;
xinit << 0.0,0.0,0.0,0.0;
xDes << 1.0,0.0,0.0,0.0;
xinit << 1.0,0.0,0.0,0.0;
xDes << 2.0,0.0,0.0,0.0;
unsigned int T = 50;
double dt=1e-4;
int iterMax = 20;
double stopCrit = 1e-3;
stateVec_t* xList;
commandVec_t* uList;
ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel;
CostFunctionRomeoActuator costRomeoActuator;
......@@ -36,13 +39,19 @@ int main()
testSolverRomeoActuator.solveTrajectory();
gettimeofday(&tend,NULL);
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
uList = lastTraj.uList;
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
cout << "temps d'execution total du solveeur ";
cout << "temps d'execution total du solveur ";
cout << texec << endl;
cout << "temps d'execution par pas de temps ";
cout << texec/T << endl;
for(int i=0;i<T+1;i++) cout << uList[i];
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