Commit 83f2990f authored by flforget's avatar flforget
Browse files

[major] fix a bug

the use of x and xDes was creating a computation problem
It is now necessary to use only x = xCurrent-xDes in DDP
parent b0b07ef9
......@@ -16,8 +16,8 @@ private:
protected:
// methods //
public:
virtual void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)=0;
virtual void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0;
virtual void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)=0;
virtual void computeFinalCostDeriv(const stateVec_t& X)=0;
private:
protected:
// accessors //
......
......@@ -31,8 +31,8 @@ private:
protected:
// methods //
public:
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes);
void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X);
private:
protected:
// accessors //
......
......@@ -71,9 +71,9 @@ private:
protected:
// methods //
public:
void FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
void FirstInitSolver(stateVec_t& myxInit, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit);
void initSolver(stateVec_t& myxInit, stateVec_t& myxDes);
void initSolver(stateVec_t& myxInit);
void solveTrajectory();
void initTrajectory();
void backwardLoop();
......
......@@ -15,17 +15,17 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
lx.setZero();
}
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)
void CostFunctionRomeoActuator::computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)
{
// lx = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0));
lx(0,0) = 100.0*X(0,0);
lu = R*U;
}
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)
void CostFunctionRomeoActuator::computeFinalCostDeriv(const stateVec_t& X)
{
// lx = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0));
lx(0,0) = 100.0*X(0,0);
}
// accessors //
......
......@@ -15,11 +15,10 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
commandNb = myDynamicModel.getCommandNb();
}
void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit)
{
xInit = myxInit;
xDes = myxDes;
T = myT;
dt = mydt;
iterMax = myiterMax;
......@@ -42,10 +41,9 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign
alpha = 1.0;
}
void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
void ILQRSolver::initSolver(stateVec_t& myxInit)
{
xInit = myxInit;
xDes = myxDes;
}
void ILQRSolver::solveTrajectory()
......@@ -82,10 +80,16 @@ void ILQRSolver::initTrajectory()
void ILQRSolver::backwardLoop()
{
costFunction->computeFinalCostDeriv(xList[T],xDes);
costFunction->computeFinalCostDeriv(xList[T]);
nextVx = costFunction->getlx();
nextVxx = costFunction->getlxx();
cout << nextVx << endl;
cout << "-" << endl;
cout << nextVxx << endl;
cout << "-" << endl;
cout << "-" << endl;
mu = 0.0;
completeBackwardFlag = 0;
......@@ -99,7 +103,7 @@ void ILQRSolver::backwardLoop()
u = uList[i];
dynamicModel->computeAllModelDeriv(dt,x,u);
costFunction->computeAllCostDeriv(x,xDes,u);
costFunction->computeAllCostDeriv(x,u);
Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
......@@ -114,9 +118,6 @@ void ILQRSolver::backwardLoop()
if(!isQuudefinitePositive(Quu))
{
/*
To be Implemented : Regularization (is Quu definite positive ?)
*/
if(mu==0.0) mu += 1e-4;
else mu *= 10;
completeBackwardFlag = 0;
......
......@@ -21,10 +21,10 @@ int main()
double texec=0.0;
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;
unsigned int T = 30;
unsigned int T = 300;
double dt=1e-4;
unsigned int iterMax = 20;
double stopCrit = 1e-3;
......@@ -35,11 +35,11 @@ int main()
RomeoSimpleActuator romeoActuatorModel(dt);
RomeoLinearActuator romeoLinearModel(dt);
CostFunctionRomeoActuator costRomeoActuator;
ILQRSolver testSolverRomeoActuator(romeoActuatorModel,costRomeoActuator);
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
ILQRSolver testSolverRomeoActuator(romeoLinearModel,costRomeoActuator);
testSolverRomeoActuator.FirstInitSolver(xinit,T,dt,iterMax,stopCrit);
int N = 100;
int N = 1;
gettimeofday(&tbegin,NULL);
for(int i=0;i<N;i++) testSolverRomeoActuator.solveTrajectory();
gettimeofday(&tend,NULL);
......
......@@ -51,12 +51,12 @@ int main()
fichier << "tau,tauDot,q,qDot,u" << endl;
testSolverRomeoActuator.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
testSolverRomeoActuator.FirstInitSolver(xinit,T,dt,iterMax,stopCrit);
gettimeofday(&tbegin,NULL);
for(int i=0;i<M;i++)
{
testSolverRomeoActuator.initSolver(xinit,xDes);
testSolverRomeoActuator.initSolver(xinit);
testSolverRomeoActuator.solveTrajectory();
lastTraj = testSolverRomeoActuator.getLastSolvedTrajectory();
xList = lastTraj.xList;
......
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