Commit 2fe0748b authored by flforget's avatar flforget
Browse files

[major] use of Eigen::StdVector instead of tables

parent c600acd3
...@@ -2,6 +2,10 @@ ...@@ -2,6 +2,10 @@
#define CONFIG_H #define CONFIG_H
#include <Eigen/Dense> #include <Eigen/Dense>
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
#define stateSize 4 #define stateSize 4
#define commandSize 1 #define commandSize 1
...@@ -30,5 +34,8 @@ typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_commandD_t[c ...@@ -30,5 +34,8 @@ typedef Eigen::Matrix<double,commandSize,stateSize> commandR_stateC_commandD_t[c
typedef Eigen::Matrix<double,stateSize,stateSize> stateR_stateC_commandD_t[commandSize]; // stateSize x stateSize x commandSize typedef Eigen::Matrix<double,stateSize,stateSize> stateR_stateC_commandD_t[commandSize]; // stateSize x stateSize x commandSize
typedef Eigen::Matrix<double,commandSize,commandSize> commandR_commandC_stateD_t[stateSize]; // commandSize x commandSize x stateSize typedef Eigen::Matrix<double,commandSize,commandSize> commandR_commandC_stateD_t[stateSize]; // commandSize x commandSize x stateSize
typedef std::vector<stateVec_t> stateVecTab_t;
typedef std::vector<commandVec_t> commandVecTab_t;
typedef std::vector<commandR_stateC_t> commandR_stateC_tab_t;
#endif // CONFIG_H #endif // CONFIG_H
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
#include "dynamicmodel.h" #include "dynamicmodel.h"
#include "costfunction.h" #include "costfunction.h"
#include <Eigen/Dense> #include <Eigen/Dense>
#include <Eigen/StdVector>
#include <qpOASES.hpp> #include <qpOASES.hpp>
#include <qpOASES/QProblemB.hpp> #include <qpOASES/QProblemB.hpp>
...@@ -20,8 +21,8 @@ class ILQRSolver ...@@ -20,8 +21,8 @@ class ILQRSolver
public: public:
struct traj struct traj
{ {
stateVec_t* xList; stateVecTab_t xList;
commandVec_t* uList; commandVecTab_t uList;
unsigned int iter; unsigned int iter;
}; };
...@@ -47,10 +48,12 @@ private: ...@@ -47,10 +48,12 @@ private:
double stopCrit; double stopCrit;
double changeAmount; double changeAmount;
stateVec_t* xList; stateVecTab_t xList;
commandVec_t* uList; commandVecTab_t uList;
stateVec_t* updatedxList; stateVecTab_t updatedxList;
commandVec_t* updateduList; commandVecTab_t updateduList;
stateVecTab_t tmpxPtr;
commandVecTab_t tmpuPtr;
struct traj lastTraj; struct traj lastTraj;
stateVec_t nextVx; stateVec_t nextVx;
...@@ -63,8 +66,8 @@ private: ...@@ -63,8 +66,8 @@ private:
commandR_stateC_t Qux; commandR_stateC_t Qux;
commandVec_t k; commandVec_t k;
commandR_stateC_t K; commandR_stateC_t K;
commandVec_t* kList; commandVecTab_t kList;
commandR_stateC_t* KList; commandR_stateC_tab_t KList;
double alphaList[5]; double alphaList[5];
double alpha; double alpha;
......
...@@ -41,14 +41,25 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign ...@@ -41,14 +41,25 @@ void ILQRSolver::FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes, unsign
iterMax = myiterMax; iterMax = myiterMax;
stopCrit = mystopCrit; stopCrit = mystopCrit;
xList = new stateVec_t[myT+1]; xList.resize(myT+1);
uList.resize(myT);
updatedxList.resize(myT+1);
updateduList.resize(myT);
tmpxPtr.resize(myT+1);
tmpuPtr.resize(myT);
k.setZero();
K.setZero();
kList.resize(myT);
KList.resize(myT);
/*xList = new stateVec_t[myT+1];
uList = new commandVec_t[myT]; uList = new commandVec_t[myT];
updatedxList = new stateVec_t[myT+1]; updatedxList = new stateVec_t[myT+1];
updateduList = new commandVec_t[myT]; updateduList = new commandVec_t[myT];
k.setZero(); k.setZero();
K.setZero(); K.setZero();
kList = new commandVec_t[myT]; kList = new commandVec_t[myT];
KList = new commandR_stateC_t[myT]; KList = new commandR_stateC_t[myT];*/
alphaList[0] = 1.0; alphaList[0] = 1.0;
alphaList[1] = 0.8; alphaList[1] = 0.8;
...@@ -66,8 +77,6 @@ void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes) ...@@ -66,8 +77,6 @@ void ILQRSolver::initSolver(stateVec_t& myxInit, stateVec_t& myxDes)
void ILQRSolver::solveTrajectory() void ILQRSolver::solveTrajectory()
{ {
stateVec_t* tmpxPtr;
commandVec_t* tmpuPtr;
initTrajectory(); initTrajectory();
for(iter=0;iter<iterMax;iter++) for(iter=0;iter<iterMax;iter++)
{ {
...@@ -160,7 +169,6 @@ void ILQRSolver::backwardLoop() ...@@ -160,7 +169,6 @@ void ILQRSolver::backwardLoop()
K.row(i_cmd).setZero(); K.row(i_cmd).setZero();
} }
} }
cout << K << endl<<endl;
} }
else else
{ {
...@@ -199,10 +207,6 @@ void ILQRSolver::forwardLoop() ...@@ -199,10 +207,6 @@ void ILQRSolver::forwardLoop()
ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory() ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
{ {
/* Debug */
//int k;
//for(k=0;k<T;k++) cout << updateduList[k] << '\t';
/**/
lastTraj.xList = updatedxList; lastTraj.xList = updatedxList;
for(int i=0;i<T+1;i++)lastTraj.xList[i] += xDes; for(int i=0;i<T+1;i++)lastTraj.xList[i] += xDes;
lastTraj.uList = updateduList; lastTraj.uList = updateduList;
......
...@@ -28,8 +28,8 @@ int main() ...@@ -28,8 +28,8 @@ int main()
double dt=1e-4; double dt=1e-4;
unsigned int iterMax = 20; unsigned int iterMax = 20;
double stopCrit = 1e-5; double stopCrit = 1e-5;
stateVec_t* xList; stateVecTab_t xList;
commandVec_t* uList; commandVecTab_t uList;
ILQRSolver::traj lastTraj; ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel(dt); RomeoSimpleActuator romeoActuatorModel(dt);
......
...@@ -30,8 +30,8 @@ int main() ...@@ -30,8 +30,8 @@ int main()
double dt=1e-4; double dt=1e-4;
unsigned int iterMax = 20; unsigned int iterMax = 20;
double stopCrit = 1e-3; double stopCrit = 1e-3;
stateVec_t* xList; stateVecTab_t xList;
commandVec_t* uList; commandVecTab_t uList;
ILQRSolver::traj lastTraj; ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel(dt); RomeoSimpleActuator romeoActuatorModel(dt);
......
...@@ -34,8 +34,8 @@ int main() ...@@ -34,8 +34,8 @@ int main()
double dt=1e-4; double dt=1e-4;
unsigned int iterMax = 50; unsigned int iterMax = 50;
double stopCrit = 1e-5; double stopCrit = 1e-5;
stateVec_t* xList; stateVecTab_t xList;
commandVec_t* uList; commandVecTab_t uList;
ILQRSolver::traj lastTraj; ILQRSolver::traj lastTraj;
RomeoSimpleActuator romeoActuatorModel(dt); RomeoSimpleActuator romeoActuatorModel(dt);
......
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