Commit e6bbaa6c authored by flforget's avatar flforget
Browse files

WOP

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