Commit 6b37650a authored by flforget's avatar flforget
Browse files

[Major] fix bug

parent 1e6d36c8
...@@ -16,8 +16,8 @@ private: ...@@ -16,8 +16,8 @@ private:
protected: protected:
// methods // // methods //
public: public:
virtual void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U)=0; virtual void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U)=0;
virtual void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes)=0; virtual void computeFinalCostDeriv(const stateVec_t& X)=0;
private: private:
protected: protected:
// accessors // // accessors //
......
...@@ -31,8 +31,8 @@ private: ...@@ -31,8 +31,8 @@ private:
protected: protected:
// methods // // methods //
public: public:
void computeAllCostDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U); void computeAllCostDeriv(const stateVec_t& X, const commandVec_t& U);
void computeFinalCostDeriv(const stateVec_t& X, const stateVec_t& Xdes); void computeFinalCostDeriv(const stateVec_t& X);
private: private:
protected: protected:
// accessors // // accessors //
......
...@@ -24,8 +24,8 @@ protected: ...@@ -24,8 +24,8 @@ protected:
// methods // // methods //
public: public:
virtual stateVec_t computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)=0; virtual stateVec_t computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)=0;
virtual void computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)=0; virtual void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)=0;
virtual stateMat_t computeTensorContxx(const stateVec_t& nextVx)=0; virtual stateMat_t computeTensorContxx(const stateVec_t& nextVx)=0;
virtual commandMat_t computeTensorContuu(const stateVec_t& nextVx)=0; virtual commandMat_t computeTensorContuu(const stateVec_t& nextVx)=0;
virtual commandR_stateC_t computeTensorContux(const stateVec_t& nextVx)=0; virtual commandR_stateC_t computeTensorContux(const stateVec_t& nextVx)=0;
......
...@@ -46,8 +46,8 @@ private: ...@@ -46,8 +46,8 @@ private:
protected: protected:
// methods // // methods //
public: public:
stateVec_t computeNextState(double& dt,const stateVec_t& X,const commandVec_t &U); stateVec_t computeNextState(double& dt,const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t &U);
void computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t &U); void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t &U);
stateMat_t computeTensorContxx(const stateVec_t& nextVx); stateMat_t computeTensorContxx(const stateVec_t& nextVx);
commandMat_t computeTensorContuu(const stateVec_t& nextVx); commandMat_t computeTensorContuu(const stateVec_t& nextVx);
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx); commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
......
...@@ -29,6 +29,7 @@ private: ...@@ -29,6 +29,7 @@ private:
static const double Cf0=0.1; static const double Cf0=0.1;
static const double a=10.0; static const double a=10.0;
stateVec_t Xreal;
stateMat_t Id; stateMat_t Id;
stateMat_t A; stateMat_t A;
stateMat_t Ad; stateMat_t Ad;
...@@ -50,8 +51,8 @@ private: ...@@ -50,8 +51,8 @@ private:
protected: protected:
// methods // // methods //
public: public:
stateVec_t computeNextState(double& dt, const stateVec_t& X, const commandVec_t &U); stateVec_t computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t &U);
void computeAllModelDeriv(double& dt, const stateVec_t& X, const commandVec_t &U); void computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t &U);
stateMat_t computeTensorContxx(const stateVec_t& nextVx); stateMat_t computeTensorContxx(const stateVec_t& nextVx);
commandMat_t computeTensorContuu(const stateVec_t& nextVx); commandMat_t computeTensorContuu(const stateVec_t& nextVx);
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx); commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
......
...@@ -15,17 +15,17 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator() ...@@ -15,17 +15,17 @@ CostFunctionRomeoActuator::CostFunctionRomeoActuator()
lx.setZero(); 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 = 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; 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 = Q*(X-Xdes);
lx(0,0) = 100.0*(X(0,0)-Xdes(0,0)); lx(0,0) = 100.0*X(0,0);
} }
// accessors // // accessors //
......
...@@ -18,7 +18,7 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio ...@@ -18,7 +18,7 @@ ILQRSolver::ILQRSolver(DynamicModel& myDynamicModel, CostFunction& myCostFunctio
void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT, void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsigned int& myT,
double& mydt, unsigned int& myiterMax,double& mystopCrit) double& mydt, unsigned int& myiterMax,double& mystopCrit)
{ {
xInit = myxInit; xInit = myxInit-myxDes;
xDes = myxDes; xDes = myxDes;
T = myT; T = myT;
dt = mydt; dt = mydt;
...@@ -76,13 +76,13 @@ void ILQRSolver::initTrajectory() ...@@ -76,13 +76,13 @@ void ILQRSolver::initTrajectory()
for(unsigned int i=0;i<T;i++) for(unsigned int i=0;i<T;i++)
{ {
uList[i] = zeroCommand; uList[i] = zeroCommand;
xList[i+1] = dynamicModel->computeNextState(dt,xList[i],zeroCommand); xList[i+1] = dynamicModel->computeNextState(dt,xList[i],xDes,zeroCommand);
} }
} }
void ILQRSolver::backwardLoop() void ILQRSolver::backwardLoop()
{ {
costFunction->computeFinalCostDeriv(xList[T],xDes); costFunction->computeFinalCostDeriv(xList[T]);
nextVx = costFunction->getlx(); nextVx = costFunction->getlx();
nextVxx = costFunction->getlxx(); nextVxx = costFunction->getlxx();
...@@ -98,8 +98,8 @@ void ILQRSolver::backwardLoop() ...@@ -98,8 +98,8 @@ void ILQRSolver::backwardLoop()
x = xList[i]; x = xList[i];
u = uList[i]; u = uList[i];
dynamicModel->computeAllModelDeriv(dt,x,u); dynamicModel->computeAllModelDeriv(dt,x,xDes,u);
costFunction->computeAllCostDeriv(x,xDes,u); costFunction->computeAllCostDeriv(x,u);
Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx; Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx; Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
...@@ -145,7 +145,7 @@ void ILQRSolver::forwardLoop() ...@@ -145,7 +145,7 @@ void ILQRSolver::forwardLoop()
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]);
updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],updateduList[i]); updatedxList[i+1] = dynamicModel->computeNextState(dt,updatedxList[i],xDes,updateduList[i]);
for(unsigned int j=0;j<commandNb;j++) for(unsigned int j=0;j<commandNb;j++)
{ {
changeAmount += abs(uList[i](j,0) - updateduList[i](j,0)); changeAmount += abs(uList[i](j,0) - updateduList[i](j,0));
...@@ -156,6 +156,7 @@ void ILQRSolver::forwardLoop() ...@@ -156,6 +156,7 @@ void ILQRSolver::forwardLoop()
ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory() ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
{ {
lastTraj.xList = updatedxList; lastTraj.xList = updatedxList;
for(int i=0;i<T+1;i++)lastTraj.xList[i] += xDes;
lastTraj.uList = updateduList; lastTraj.uList = updateduList;
lastTraj.iter = iter; lastTraj.iter = iter;
return lastTraj; return lastTraj;
......
...@@ -42,13 +42,13 @@ RomeoLinearActuator::RomeoLinearActuator(double& mydt) ...@@ -42,13 +42,13 @@ RomeoLinearActuator::RomeoLinearActuator(double& mydt)
fxu[0].setZero(); fxu[0].setZero();
} }
stateVec_t RomeoLinearActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U) stateVec_t RomeoLinearActuator::computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
{ {
stateVec_t result = Ad*X + Bd*U; stateVec_t result = Ad*(X+Xdes) + Bd*U;
return result; return result;
} }
void RomeoLinearActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U) void RomeoLinearActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
{ {
} }
......
...@@ -53,22 +53,23 @@ RomeoSimpleActuator::RomeoSimpleActuator(double& mydt) ...@@ -53,22 +53,23 @@ RomeoSimpleActuator::RomeoSimpleActuator(double& mydt)
} }
stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U) stateVec_t RomeoSimpleActuator::computeNextState(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
{ {
stateVec_t result = Ad*X + Bd*U; stateVec_t result = Ad*X + Bd*U;
result(1,0)+=A13atan*atan(a*X(3,0)); result(1,0)+=A13atan*atan(a*(X(3,0)+Xdes(3,0)));
result(3,0)+=A33atan*atan(a*X(3,0)); result(3,0)+=A33atan*atan(a*(X(3,0)+Xdes(3,0)));
return result; return result;
} }
void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U) void RomeoSimpleActuator::computeAllModelDeriv(double& dt, const stateVec_t& X,const stateVec_t& Xdes,const commandVec_t& U)
{ {
Xreal = X + Xdes;
fx = fxBase; fx = fxBase;
fx(1,3) += A13atan*(a/(1+a*a*X(3,0)*X(3,0))); fx(1,3) += A13atan*(a/(1+a*a*Xreal(3,0)*Xreal(3,0)));
fx(3,3) -= A33atan*(a/(1+(a*a*X(3,0)*X(3,0)))); fx(3,3) -= A33atan*(a/(1+(a*a*Xreal(3,0)*Xreal(3,0))));
fxx[3](1,3) = -((2*dt*Jm*R)/(pi*Jl))*Cf0*((2*a*a*a*X(3,0))/((1+(a*a*X(3,0)*X(3,0)))*(1+(a*a*X(3,0)*X(3,0))))); fxx[3](1,3) = -((2*dt*Jm*R)/(pi*Jl))*Cf0*((2*a*a*a*Xreal(3,0))/((1+(a*a*Xreal(3,0)*Xreal(3,0)))*(1+(a*a*Xreal(3,0)*Xreal(3,0)))));
fxx[3](3,3) = +((2*dt*Cf0)/(pi*Jl))*((2*a*a*a*X(3,0))/((1+(a*a*X(3,0)*X(3,0)))*(1+(a*a*X(3,0)*X(3,0))))); fxx[3](3,3) = +((2*dt*Cf0)/(pi*Jl))*((2*a*a*a*Xreal(3,0))/((1+(a*a*Xreal(3,0)*Xreal(3,0)))*(1+(a*a*Xreal(3,0)*Xreal(3,0)))));
} }
stateMat_t RomeoSimpleActuator::computeTensorContxx(const stateVec_t& nextVx) stateMat_t RomeoSimpleActuator::computeTensorContxx(const stateVec_t& nextVx)
......
...@@ -21,10 +21,10 @@ int main() ...@@ -21,10 +21,10 @@ 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 = 30; unsigned int T = 300;
double dt=1e-4; double dt=1e-4;
unsigned int iterMax = 20; unsigned int iterMax = 20;
double stopCrit = 1e-3; double stopCrit = 1e-3;
......
...@@ -25,7 +25,7 @@ int main() ...@@ -25,7 +25,7 @@ int main()
xinit << 0.0,0.0,0.0,0.0; xinit << 0.0,0.0,0.0,0.0;
xDes << 1.0,0.0,0.0,0.0; xDes << 1.0,0.0,0.0,0.0;
unsigned int T = 8; unsigned int T = 300;
unsigned int M = 30; unsigned int M = 30;
double dt=1e-4; double dt=1e-4;
unsigned int iterMax = 20; unsigned int iterMax = 20;
......
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