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

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

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