Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/ddp-actuator-solver/ddpsolver.hh	Lines:	0	166	0.0 %
Date:	2023-06-02 15:50:43	Branches:	0	234	0.0 %


#ifndef DDPSOLVER_H
#define DDPSOLVER_H

#include <sys/time.h>
#include <time.h>

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <iostream>
#include <qpOASES.hpp>
#include <qpOASES/QProblemB.hpp>

#include "costfunction.hh"
#include "dynamicmodel.hh"

#define ENABLE_QPBOX 1
#define DISABLE_QPBOX 0
#define ENABLE_FULLDDP 1
#define DISABLE_FULLDDP 0

USING_NAMESPACE_QPOASES

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)

template <typename precision, int stateSize, int commandSize>
class DDPSolver {
 public:
  typedef Eigen::Matrix<precision, stateSize, 1> stateVec_t;  // 1 x stateSize
  typedef Eigen::Matrix<precision, 1, stateSize>
      stateVecTrans_t;  // 1 x stateSize
  typedef Eigen::Matrix<precision, stateSize, stateSize>
      stateMat_t;  // stateSize x stateSize

  // typedef for commandSize types
  typedef Eigen::Matrix<precision, commandSize, 1>
      commandVec_t;  // commandSize x 1
  typedef Eigen::Matrix<precision, 1, commandSize>
      commandVecTrans_t;  // 1 x commandSize
  typedef Eigen::Matrix<precision, commandSize, commandSize>
      commandMat_t;  // commandSize x commandSize

  // typedef for mixed stateSize and commandSize types
  typedef Eigen::Matrix<precision, stateSize, commandSize>
      stateR_commandC_t;  // stateSize x commandSize
  typedef Eigen::Matrix<precision, stateSize, commandSize>
      stateR_commandC_stateD_t[stateSize];  // stateSize x commandSize x
                                            // stateSize
  typedef Eigen::Matrix<precision, stateSize, commandSize>
      stateR_commandC_commandD_t[commandSize];  // stateSize x commandSize x
                                                // commandSize
  typedef Eigen::Matrix<precision, commandSize, stateSize>
      commandR_stateC_t;  // commandSize x stateSize
  typedef Eigen::Matrix<precision, commandSize, stateSize>
      commandR_stateC_stateD_t[stateSize];  // commandSize x stateSize x
                                            // stateSize
  typedef Eigen::Matrix<precision, commandSize, stateSize>
      commandR_stateC_commandD_t[commandSize];  // commandSize x stateSize x
                                                // commandSize
  typedef Eigen::Matrix<precision, stateSize, stateSize>
      stateR_stateC_commandD_t[commandSize];  // stateSize x stateSize x
                                              // commandSize
  typedef Eigen::Matrix<precision, 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;

  typedef DynamicModel<precision, stateSize, commandSize> DynamicModel_t;
  typedef CostFunction<precision, stateSize, commandSize> CostFunction_t;

 public:
  struct traj {
    stateVecTab_t xList;
    commandVecTab_t uList;
    unsigned int iter;
  };

 public:
 private:
 protected:
  // attributes //
 public:
 private:
  DynamicModel_t* dynamicModel;
  CostFunction_t* costFunction;
  unsigned int stateNb;
  unsigned int commandNb;
  stateVec_t x;
  commandVec_t u;
  stateVec_t xInit;
  stateVec_t xDes;
  unsigned int T;
  unsigned int iter;
  double dt;
  unsigned int iterMax;
  double stopCrit;
  double changeAmount;
  double cost, previous_cost;
  commandVec_t zeroCommand;

  stateVecTab_t xList;
  commandVecTab_t uList;
  stateVecTab_t updatedxList;
  commandVecTab_t updateduList;
  stateVecTab_t tmpxPtr;
  commandVecTab_t tmpuPtr;
  struct traj lastTraj;

  stateVec_t nextVx;
  stateMat_t nextVxx;
  stateVec_t Qx;
  stateMat_t Qxx;
  commandVec_t Qu;
  commandMat_t Quu, Quu_reg;
  Eigen::LLT<commandMat_t> lltofQuu;
  commandMat_t QuuInv;
  commandR_stateC_t Qux, Qux_reg;
  commandVec_t k;
  commandR_stateC_t K;
  commandVecTab_t kList;
  commandR_stateC_tab_t KList;
  double alphaList[5];
  double alpha;

  double mu;
  stateMat_t muEye;
  unsigned char completeBackwardFlag;

  /* QP variables */
  QProblemB* qp;
  bool enableQPBox;
  bool enableFullDDP;
  commandMat_t H;
  commandVec_t g;
  commandVec_t lowerCommandBounds;
  commandVec_t upperCommandBounds;
  commandVec_t lb;
  commandVec_t ub;
  int nWSR;
  real_t* xOpt;

 protected:
  // methods //
 public:
  DDPSolver(DynamicModel_t& myDynamicModel, CostFunction_t& myCostFunction,
            bool fullDDP = 0, bool QPBox = 0) {
    dynamicModel = &myDynamicModel;
    costFunction = &myCostFunction;
    stateNb = myDynamicModel.getStateNb();
    commandNb = myDynamicModel.getCommandNb();
    enableQPBox = QPBox;
    enableFullDDP = fullDDP;
    zeroCommand.setZero();
    cost = 0;
    previous_cost = 0;
    if (QPBox) {
      qp = new QProblemB(commandNb);
      Options myOptions;
      myOptions.printLevel = PL_LOW;
      myOptions.enableRegularisation = BT_TRUE;
      myOptions.initialStatusBounds = ST_INACTIVE;
      myOptions.numRefinementSteps = 1;
      myOptions.enableCholeskyRefactorisation = 1;
      qp->setOptions(myOptions);

      xOpt = new real_t[commandNb];
      lowerCommandBounds = myDynamicModel.getLowerCommandBounds();
      upperCommandBounds = myDynamicModel.getUpperCommandBounds();
    }
  }

  void FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes,
                       unsigned int& myT, double& mydt, unsigned int& myiterMax,
                       double& mystopCrit) {
    xInit = myxInit;
    xDes = myxDes;
    T = myT;
    dt = mydt;
    iterMax = myiterMax;
    stopCrit = mystopCrit;

    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);

    alphaList[0] = 1.0;
    alphaList[1] = 0.8;
    alphaList[2] = 0.6;
    alphaList[3] = 0.4;
    alphaList[4] = 0.2;
    alpha = 1.0;
  }

  void initSolver(stateVec_t& myxInit, stateVec_t& myxDes) {
    xInit = myxInit;
    xDes = myxDes;
  }

  commandVec_t solveTrajectory() {
    initTrajectory();
    for (iter = 1; iter < iterMax; iter++) {
      backwardLoop();
      forwardLoop();
      if (changeAmount < stopCrit) {
        break;
      }
      tmpxPtr = xList;
      tmpuPtr = uList;
      xList = updatedxList;
      updatedxList = tmpxPtr;
      uList = updateduList;
      updateduList = tmpuPtr;
    }
    return updateduList[0];
  }

  void initTrajectory() {
    xList[0] = xInit;
    for (unsigned int i = 0; i < T; i++) {
      uList[i] = zeroCommand;
      xList[i + 1] = dynamicModel->computeNextState(dt, xList[i], zeroCommand);
    }
  }

  void backwardLoop() {
    costFunction->computeFinalCostAndDeriv(xList[T], xDes);
    cost = costFunction->getFinalCost();
    nextVx = costFunction->getlx();
    nextVxx = costFunction->getlxx();

    mu = 0.0;
    completeBackwardFlag = 0;

    while (!completeBackwardFlag) {
      completeBackwardFlag = 1;
      muEye = stateMat_t::Constant(mu);
      for (int i = T - 1; i >= 0; i--) {
        x = xList[i];
        u = uList[i];

        dynamicModel->computeModelDeriv(dt, x, u);
        costFunction->computeCostAndDeriv(x, xDes, u);
        cost += costFunction->getRunningCost();

        Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
        Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
        Qxx = costFunction->getlxx() + dynamicModel->getfx().transpose() *
                                           (nextVxx)*dynamicModel->getfx();
        Quu = costFunction->getluu() + dynamicModel->getfu().transpose() *
                                           (nextVxx)*dynamicModel->getfu();
        Qux = costFunction->getlux() + dynamicModel->getfu().transpose() *
                                           (nextVxx)*dynamicModel->getfx();
        Quu_reg = costFunction->getluu() + dynamicModel->getfu().transpose() *
                                               (nextVxx + muEye) *
                                               dynamicModel->getfu();
        Qux_reg = costFunction->getlux() + dynamicModel->getfu().transpose() *
                                               (nextVxx + muEye) *
                                               dynamicModel->getfx();

        if (enableFullDDP) {
          Qxx += dynamicModel->computeTensorContxx(nextVx);
          Qux += dynamicModel->computeTensorContux(nextVx);
          Quu += dynamicModel->computeTensorContuu(nextVx);
          Qux_reg += dynamicModel->computeTensorContux(nextVx);
          Quu_reg += dynamicModel->computeTensorContuu(nextVx);
        }

        if (!isQuudefinitePositive(Quu_reg)) {
          std::cout << "regularization" << std::endl;  // to remove
          if (mu == 0.0)
            mu += 1e-4;
          else
            mu *= 10;
          completeBackwardFlag = 0;
          break;
        }

        QuuInv = Quu.inverse();

        if (enableQPBox) {
          nWSR = 10;
          H = Quu_reg;
          g = Qu;
          lb = lowerCommandBounds - u;
          ub = upperCommandBounds - u;
          qp->init(H.data(), g.data(), lb.data(), ub.data(), nWSR);
          qp->getPrimalSolution(xOpt);
          k = Eigen::Map<commandVec_t>(xOpt);
          K = -QuuInv * Qux;
          for (unsigned int i_cmd = 0; i_cmd < commandNb; i_cmd++) {
            if ((k[i_cmd] == lowerCommandBounds[i_cmd]) |
                (k[i_cmd] == upperCommandBounds[i_cmd])) {
              K.row(i_cmd).setZero();
            }
          }
        } else {
          k = -QuuInv * Qu;
          K = -QuuInv * Qux;
        }

        /*nextVx = Qx - K.transpose()*Quu*k;
         nextVxx = Qxx - K.transpose()*Quu*K;*/
        nextVx = Qx + K.transpose() * Quu * k + K.transpose() * Qu +
                 Qux.transpose() * k;
        nextVxx = Qxx + K.transpose() * Quu * K + K.transpose() * Qux +
                  Qux.transpose() * K;
        nextVxx = 0.5 * (nextVxx + nextVxx.transpose());

        kList[i] = k;
        KList[i] = K;
      }
    }
  }

  void forwardLoop() {
    changeAmount = 0.0;
    updatedxList[0] = xInit;
    // Line search to be implemented
    alpha = 1.0;
    for (unsigned int i = 0; i < T; i++) {
      updateduList[i] =
          uList[i] + alpha * kList[i] + KList[i] * (updatedxList[i] - xList[i]);
      updatedxList[i + 1] =
          dynamicModel->computeNextState(dt, updatedxList[i], updateduList[i]);
      changeAmount = fabs(previous_cost - cost) / cost;
    }
    previous_cost = cost;
  }

  DDPSolver::traj getLastSolvedTrajectory() {
    lastTraj.xList = updatedxList;
    lastTraj.uList = updateduList;
    lastTraj.iter = iter;
    return lastTraj;
  }

  DDPSolver::commandVec_t getLastCommand() { return updateduList[0]; }

  bool isQuudefinitePositive(const commandMat_t& Quu_reg) {
    lltofQuu.compute(Quu_reg);
    if (lltofQuu.info() == Eigen::NumericalIssue) {
      std::cout << "not sdp" << std::endl;
      std::cout << "#############################" << std::endl;
      std::cout << "Quu_reg : " << Quu_reg << std::endl;
      std::cout << "lxx : " << costFunction->getlxx() << std::endl;
      std::cout << "lu : " << costFunction->getlu() << std::endl;
      std::cout << "lx : " << costFunction->getlx() << std::endl;
      std::cout << "luu" << costFunction->getluu() << std::endl;
      std::cout << "updateduList[0] : " << updateduList[0] << std::endl;
      std::cout << "updatedxList[0] : " << updatedxList[0] << std::endl;
      std::cout << "#############################" << std::endl;
      return false;
    }
    return true;
  }

 protected:
};

#endif  // DDPSOLVER_H


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#ifndef DDPSOLVER_H
2			#define DDPSOLVER_H
3
4			#include <sys/time.h>
5			#include <time.h>
6
7			#include <Eigen/Dense>
8			#include <Eigen/StdVector>
9			#include <iostream>
10			#include <qpOASES.hpp>
11			#include <qpOASES/QProblemB.hpp>
12
13			#include "costfunction.hh"
14			#include "dynamicmodel.hh"
15
16			#define ENABLE_QPBOX 1
17			#define DISABLE_QPBOX 0
18			#define ENABLE_FULLDDP 1
19			#define DISABLE_FULLDDP 0
20
21			USING_NAMESPACE_QPOASES
22
23			EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXd)
24			EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
25
26			template <typename precision, int stateSize, int commandSize>
27			class DDPSolver {
28			public:
29			typedef Eigen::Matrix<precision, stateSize, 1> stateVec_t; // 1 x stateSize
30			typedef Eigen::Matrix<precision, 1, stateSize>
31			stateVecTrans_t; // 1 x stateSize
32			typedef Eigen::Matrix<precision, stateSize, stateSize>
33			stateMat_t; // stateSize x stateSize
34
35			// typedef for commandSize types
36			typedef Eigen::Matrix<precision, commandSize, 1>
37			commandVec_t; // commandSize x 1
38			typedef Eigen::Matrix<precision, 1, commandSize>
39			commandVecTrans_t; // 1 x commandSize
40			typedef Eigen::Matrix<precision, commandSize, commandSize>
41			commandMat_t; // commandSize x commandSize
42
43			// typedef for mixed stateSize and commandSize types
44			typedef Eigen::Matrix<precision, stateSize, commandSize>
45			stateR_commandC_t; // stateSize x commandSize
46			typedef Eigen::Matrix<precision, stateSize, commandSize>
47			stateR_commandC_stateD_t[stateSize]; // stateSize x commandSize x
48			// stateSize
49			typedef Eigen::Matrix<precision, stateSize, commandSize>
50			stateR_commandC_commandD_t[commandSize]; // stateSize x commandSize x
51			// commandSize
52			typedef Eigen::Matrix<precision, commandSize, stateSize>
53			commandR_stateC_t; // commandSize x stateSize
54			typedef Eigen::Matrix<precision, commandSize, stateSize>
55			commandR_stateC_stateD_t[stateSize]; // commandSize x stateSize x
56			// stateSize
57			typedef Eigen::Matrix<precision, commandSize, stateSize>
58			commandR_stateC_commandD_t[commandSize]; // commandSize x stateSize x
59			// commandSize
60			typedef Eigen::Matrix<precision, stateSize, stateSize>
61			stateR_stateC_commandD_t[commandSize]; // stateSize x stateSize x
62			// commandSize
63			typedef Eigen::Matrix<precision, commandSize, commandSize>
64			commandR_commandC_stateD_t[stateSize]; // commandSize x commandSize x
65			// stateSize
66
67			typedef std::vector<stateVec_t> stateVecTab_t;
68			typedef std::vector<commandVec_t> commandVecTab_t;
69			typedef std::vector<commandR_stateC_t> commandR_stateC_tab_t;
70
71			typedef DynamicModel<precision, stateSize, commandSize> DynamicModel_t;
72			typedef CostFunction<precision, stateSize, commandSize> CostFunction_t;
73
74			public:
75			struct traj {
76			stateVecTab_t xList;
77			commandVecTab_t uList;
78			unsigned int iter;
79			};
80
81			public:
82			private:
83			protected:
84			// attributes //
85			public:
86			private:
87			DynamicModel_t* dynamicModel;
88			CostFunction_t* costFunction;
89			unsigned int stateNb;
90			unsigned int commandNb;
91			stateVec_t x;
92			commandVec_t u;
93			stateVec_t xInit;
94			stateVec_t xDes;
95			unsigned int T;
96			unsigned int iter;
97			double dt;
98			unsigned int iterMax;
99			double stopCrit;
100			double changeAmount;
101			double cost, previous_cost;
102			commandVec_t zeroCommand;
103
104			stateVecTab_t xList;
105			commandVecTab_t uList;
106			stateVecTab_t updatedxList;
107			commandVecTab_t updateduList;
108			stateVecTab_t tmpxPtr;
109			commandVecTab_t tmpuPtr;
110			struct traj lastTraj;
111
112			stateVec_t nextVx;
113			stateMat_t nextVxx;
114			stateVec_t Qx;
115			stateMat_t Qxx;
116			commandVec_t Qu;
117			commandMat_t Quu, Quu_reg;
118			Eigen::LLT<commandMat_t> lltofQuu;
119			commandMat_t QuuInv;
120			commandR_stateC_t Qux, Qux_reg;
121			commandVec_t k;
122			commandR_stateC_t K;
123			commandVecTab_t kList;
124			commandR_stateC_tab_t KList;
125			double alphaList[5];
126			double alpha;
127
128			double mu;
129			stateMat_t muEye;
130			unsigned char completeBackwardFlag;
131
132			/* QP variables */
133			QProblemB* qp;
134			bool enableQPBox;
135			bool enableFullDDP;
136			commandMat_t H;
137			commandVec_t g;
138			commandVec_t lowerCommandBounds;
139			commandVec_t upperCommandBounds;
140			commandVec_t lb;
141			commandVec_t ub;
142			int nWSR;
143			real_t* xOpt;
144
145			protected:
146			// methods //
147			public:
148			DDPSolver(DynamicModel_t& myDynamicModel, CostFunction_t& myCostFunction,
149			bool fullDDP = 0, bool QPBox = 0) {
150			dynamicModel = &myDynamicModel;
151			costFunction = &myCostFunction;
152			stateNb = myDynamicModel.getStateNb();
153			commandNb = myDynamicModel.getCommandNb();
154			enableQPBox = QPBox;
155			enableFullDDP = fullDDP;
156			zeroCommand.setZero();
157			cost = 0;
158			previous_cost = 0;
159			if (QPBox) {
160			qp = new QProblemB(commandNb);
161			Options myOptions;
162			myOptions.printLevel = PL_LOW;
163			myOptions.enableRegularisation = BT_TRUE;
164			myOptions.initialStatusBounds = ST_INACTIVE;
165			myOptions.numRefinementSteps = 1;
166			myOptions.enableCholeskyRefactorisation = 1;
167			qp->setOptions(myOptions);
168
169			xOpt = new real_t[commandNb];
170			lowerCommandBounds = myDynamicModel.getLowerCommandBounds();
171			upperCommandBounds = myDynamicModel.getUpperCommandBounds();
172			}
173			}
174
175			void FirstInitSolver(stateVec_t& myxInit, stateVec_t& myxDes,
176			unsigned int& myT, double& mydt, unsigned int& myiterMax,
177			double& mystopCrit) {
178			xInit = myxInit;
179			xDes = myxDes;
180			T = myT;
181			dt = mydt;
182			iterMax = myiterMax;
183			stopCrit = mystopCrit;
184
185			xList.resize(myT + 1);
186			uList.resize(myT);
187			updatedxList.resize(myT + 1);
188			updateduList.resize(myT);
189			tmpxPtr.resize(myT + 1);
190			tmpuPtr.resize(myT);
191			k.setZero();
192			K.setZero();
193			kList.resize(myT);
194			KList.resize(myT);
195
196			alphaList[0] = 1.0;
197			alphaList[1] = 0.8;
198			alphaList[2] = 0.6;
199			alphaList[3] = 0.4;
200			alphaList[4] = 0.2;
201			alpha = 1.0;
202			}
203
204			void initSolver(stateVec_t& myxInit, stateVec_t& myxDes) {
205			xInit = myxInit;
206			xDes = myxDes;
207			}
208
209			commandVec_t solveTrajectory() {
210			initTrajectory();
211			for (iter = 1; iter < iterMax; iter++) {
212			backwardLoop();
213			forwardLoop();
214			if (changeAmount < stopCrit) {
215			break;
216			}
217			tmpxPtr = xList;
218			tmpuPtr = uList;
219			xList = updatedxList;
220			updatedxList = tmpxPtr;
221			uList = updateduList;
222			updateduList = tmpuPtr;
223			}
224			return updateduList[0];
225			}
226
227			void initTrajectory() {
228			xList[0] = xInit;
229			for (unsigned int i = 0; i < T; i++) {
230			uList[i] = zeroCommand;
231			xList[i + 1] = dynamicModel->computeNextState(dt, xList[i], zeroCommand);
232			}
233			}
234
235			void backwardLoop() {
236			costFunction->computeFinalCostAndDeriv(xList[T], xDes);
237			cost = costFunction->getFinalCost();
238			nextVx = costFunction->getlx();
239			nextVxx = costFunction->getlxx();
240
241			mu = 0.0;
242			completeBackwardFlag = 0;
243
244			while (!completeBackwardFlag) {
245			completeBackwardFlag = 1;
246			muEye = stateMat_t::Constant(mu);
247			for (int i = T - 1; i >= 0; i--) {
248			x = xList[i];
249			u = uList[i];
250
251			dynamicModel->computeModelDeriv(dt, x, u);
252			costFunction->computeCostAndDeriv(x, xDes, u);
253			cost += costFunction->getRunningCost();
254
255			Qx = costFunction->getlx() + dynamicModel->getfx().transpose() * nextVx;
256			Qu = costFunction->getlu() + dynamicModel->getfu().transpose() * nextVx;
257			Qxx = costFunction->getlxx() + dynamicModel->getfx().transpose() *
258			(nextVxx)*dynamicModel->getfx();
259			Quu = costFunction->getluu() + dynamicModel->getfu().transpose() *
260			(nextVxx)*dynamicModel->getfu();
261			Qux = costFunction->getlux() + dynamicModel->getfu().transpose() *
262			(nextVxx)*dynamicModel->getfx();
263			Quu_reg = costFunction->getluu() + dynamicModel->getfu().transpose() *
264			(nextVxx + muEye) *
265			dynamicModel->getfu();
266			Qux_reg = costFunction->getlux() + dynamicModel->getfu().transpose() *
267			(nextVxx + muEye) *
268			dynamicModel->getfx();
269
270			if (enableFullDDP) {
271			Qxx += dynamicModel->computeTensorContxx(nextVx);
272			Qux += dynamicModel->computeTensorContux(nextVx);
273			Quu += dynamicModel->computeTensorContuu(nextVx);
274			Qux_reg += dynamicModel->computeTensorContux(nextVx);
275			Quu_reg += dynamicModel->computeTensorContuu(nextVx);
276			}
277
278			if (!isQuudefinitePositive(Quu_reg)) {
279			std::cout << "regularization" << std::endl; // to remove
280			if (mu == 0.0)
281			mu += 1e-4;
282			else
283			mu *= 10;
284			completeBackwardFlag = 0;
285			break;
286			}
287
288			QuuInv = Quu.inverse();
289
290			if (enableQPBox) {
291			nWSR = 10;
292			H = Quu_reg;
293			g = Qu;
294			lb = lowerCommandBounds - u;
295			ub = upperCommandBounds - u;
296			qp->init(H.data(), g.data(), lb.data(), ub.data(), nWSR);
297			qp->getPrimalSolution(xOpt);
298			k = Eigen::Map<commandVec_t>(xOpt);
299			K = -QuuInv * Qux;
300			for (unsigned int i_cmd = 0; i_cmd < commandNb; i_cmd++) {
301			if ((k[i_cmd] == lowerCommandBounds[i_cmd]) \|
302			(k[i_cmd] == upperCommandBounds[i_cmd])) {
303			K.row(i_cmd).setZero();
304			}
305			}
306			} else {
307			k = -QuuInv * Qu;
308			K = -QuuInv * Qux;
309			}
310
311			/nextVx = Qx - K.transpose()Quu*k;
312			nextVxx = Qxx - K.transpose()QuuK;*/
313			nextVx = Qx + K.transpose() * Quu * k + K.transpose() * Qu +
314			Qux.transpose() * k;
315			nextVxx = Qxx + K.transpose() * Quu * K + K.transpose() * Qux +
316			Qux.transpose() * K;
317			nextVxx = 0.5 * (nextVxx + nextVxx.transpose());
318
319			kList[i] = k;
320			KList[i] = K;
321			}
322			}
323			}
324
325			void forwardLoop() {
326			changeAmount = 0.0;
327			updatedxList[0] = xInit;
328			// Line search to be implemented
329			alpha = 1.0;
330			for (unsigned int i = 0; i < T; i++) {
331			updateduList[i] =
332			uList[i] + alpha * kList[i] + KList[i] * (updatedxList[i] - xList[i]);
333			updatedxList[i + 1] =
334			dynamicModel->computeNextState(dt, updatedxList[i], updateduList[i]);
335			changeAmount = fabs(previous_cost - cost) / cost;
336			}
337			previous_cost = cost;
338			}
339
340			DDPSolver::traj getLastSolvedTrajectory() {
341			lastTraj.xList = updatedxList;
342			lastTraj.uList = updateduList;
343			lastTraj.iter = iter;
344			return lastTraj;
345			}
346
347			DDPSolver::commandVec_t getLastCommand() { return updateduList[0]; }
348
349			bool isQuudefinitePositive(const commandMat_t& Quu_reg) {
350			lltofQuu.compute(Quu_reg);
351			if (lltofQuu.info() == Eigen::NumericalIssue) {
352			std::cout << "not sdp" << std::endl;
353			std::cout << "#############################" << std::endl;
354			std::cout << "Quu_reg : " << Quu_reg << std::endl;
355			std::cout << "lxx : " << costFunction->getlxx() << std::endl;
356			std::cout << "lu : " << costFunction->getlu() << std::endl;
357			std::cout << "lx : " << costFunction->getlx() << std::endl;
358			std::cout << "luu" << costFunction->getluu() << std::endl;
359			std::cout << "updateduList[0] : " << updateduList[0] << std::endl;
360			std::cout << "updatedxList[0] : " << updatedxList[0] << std::endl;
361			std::cout << "#############################" << std::endl;
362			return false;
363			}
364			return true;
365			}
366
367			protected:
368			};
369
370			#endif // DDPSOLVER_H