diff --git a/include/bezier-com-traj/common_solve_methods.hh b/include/bezier-com-traj/common_solve_methods.hh
index 5cf3bf35e847175019b45af8dc3a66931303155b..9fa0347403fd4d7ffc1ca1f83c4f1d560967029a 100644
--- a/include/bezier-com-traj/common_solve_methods.hh
+++ b/include/bezier-com-traj/common_solve_methods.hh
@@ -62,10 +62,18 @@ BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointEquali
* @param b Inequality vector
* @param H Cost matrix
* @param g cost Vector
+ * @param x initGuess initial guess
+ * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x.
+ * Unbounded elements should be lesser or equal to solvers::UNBOUNDED_UP;
+ * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x
+ * Unbounded elements should be higher or lower than solvers::UNBOUNDED_DOWN;
+ * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
* @return
*/
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H,
- Cref_vectorX g, Cref_vectorX initGuess, const solvers::SolverType solver = solvers::SOLVER_QUADPROG );
+ Cref_vectorX g, Cref_vectorX initGuess,
+ Cref_vectorX minBounds, Cref_vectorX maxBounds,
+ const solvers::SolverType solver = solvers::SOLVER_QUADPROG );
/**
* @brief solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog
* @param A Inequality matrix
@@ -97,11 +105,17 @@ BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
* @param Ab Inequality matrix and vector
* @param Dd Equality matrix and vector
* @param Hg Cost matrix and vector
+ * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x.
+ * Unbounded elements should be equal to -std::numeric_limits<double>::infinity();
+ * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x
+ * Unbounded elements should be equal to std::numeric_limits<double>::infinity();
+ * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
* @return
*/
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
const std::pair<MatrixXX, VectorX>& Dd,
const std::pair<MatrixXX, VectorX>& Hg,
+ Cref_vectorX minBounds, Cref_vectorX maxBounds,
const VectorX& init, const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
template <typename Point>
diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index 6bb3074666e64b8c8494ad789c9b0b758f21475a..2e890b770c918c6540b99185493530d3d143b04e 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -46,7 +46,7 @@ namespace bezier_com_traj
* @param Ts timelength of each contact phase. Should be the same length as pData.contacts
* @param timeStep time step used by the discretization, if -1 : use the continuous fomulation
* @param solver solver used to perform optimization. WARNING: if the continuous force formulation is
- * used, it is highly recommended to use the SOLVER_QUADPROG_SPARSE solver, or the SOLVER_GLPK solver if available and a quadratic
+ * used, it is highly recommended to use the SOLVER_GLPK solver if available and a quadratic
* cost is not necessary, as these solvers are increasely more computationnaly efficient for the problem
* @return ResultData a struct containing the resulting trajectory, if success is true.
*/
diff --git a/include/solver/glpk-wrapper.hpp b/include/solver/glpk-wrapper.hpp
index 6e28433017b8c09ceb00b46d6516364018a845d6..1487984db5906619d3599a09d4cab0960681dcc7 100644
--- a/include/solver/glpk-wrapper.hpp
+++ b/include/solver/glpk-wrapper.hpp
@@ -28,11 +28,13 @@ namespace solvers
// min g'x
// st CIx <= ci0
// CEx = ce0
- int solve(const VectorXd & g0,
+ int solveglpk(const VectorXd & g0,
const MatrixXd & CE,
const VectorXd & ce0,
const MatrixXd & CI,
const VectorXd & ci0,
+ solvers::Cref_vectorX minBounds,
+ solvers::Cref_vectorX maxBounds,
VectorXd& x,
double& cost);
diff --git a/include/solver/solver-abstract.hpp b/include/solver/solver-abstract.hpp
index caa9c2ceacff4c5f2e88430111a47036f4585df9..a01ecce83f97efa9f8b2ec20e0230d947a6e11ef 100644
--- a/include/solver/solver-abstract.hpp
+++ b/include/solver/solver-abstract.hpp
@@ -33,6 +33,9 @@ enum optim_status
OPTIM_INFEASIBLE=1
};
+static const double UNBOUNDED_UP = 100000.;
+static const double UNBOUNDED_DOWN = -100000.;
+
typedef Eigen::MatrixXd MatrixXd;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::VectorXi VectorXi;
@@ -40,10 +43,10 @@ typedef const Eigen::Ref<const VectorXd> & Cref_vectorX;
enum BEZIER_COM_TRAJ_DLLAPI SolverType
{
- SOLVER_QUADPROG = 0x00001,
- SOLVER_QUADPROG_SPARSE = 0x00002
+ SOLVER_QUADPROG = 0x00001
+ //SOLVER_QUADPROG_SPARSE = 0x00002
#ifdef USE_GLPK_SOLVER
- ,SOLVER_GLPK = 0x00004
+ ,SOLVER_GLPK = 0x00002
#endif
};
@@ -100,6 +103,7 @@ ResultData BEZIER_COM_TRAJ_DLLAPI solve(const MatrixXd & A,
const MatrixXd & Hess,
const VectorXd & g,
const VectorXd & initGuess,
+ Cref_vectorX minBounds, Cref_vectorX maxBounds,
const SolverType solver);
diff --git a/python/bezier_com_traj.cpp b/python/bezier_com_traj.cpp
index 0845a4d3b68d8e662b7bf969ea8b1bb52f2329fa..adb1c6ca5a39ef36a7e00abef5ca33337ac801b5 100644
--- a/python/bezier_com_traj.cpp
+++ b/python/bezier_com_traj.cpp
@@ -443,7 +443,7 @@ BOOST_PYTHON_MODULE(bezier_com_traj)
enum_<solvers::SolverType>("SolverType")
.value("SOLVER_QUADPROG", solvers::SOLVER_QUADPROG)
- .value("SOLVER_QUADPROG_SPARSE", solvers::SOLVER_QUADPROG_SPARSE)
+ //.value("SOLVER_QUADPROG_SPARSE", solvers::SOLVER_QUADPROG_SPARSE)
#ifdef USE_GLPK_SOLVER
.value("SOLVER_GLPK", solvers::SOLVER_GLPK)
#endif
diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp
index 655e435adf888666e12c7a4a8f409cc2bb38f0df..f0400e0e7f05eadc43d1625a9c4bce0daee8787b 100644
--- a/src/common_solve_methods.cpp
+++ b/src/common_solve_methods.cpp
@@ -146,16 +146,18 @@ std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData
}
ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H,
- Cref_vectorX g, Cref_vectorX initGuess, const solvers::SolverType solver)
+ Cref_vectorX g, Cref_vectorX initGuess,
+ Cref_vectorX minBounds, Cref_vectorX maxBounds,
+ const solvers::SolverType solver)
{
- return solvers::solve(A,ci0,D,d,H,g,initGuess,solver);
+ return solvers::solve(A,ci0,D,d,H,g,initGuess,minBounds, maxBounds, solver);
}
ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, const solvers::SolverType solver)
{
MatrixXX D = MatrixXX::Zero(0,A.cols());
VectorX d = VectorX::Zero(0);
- return solvers::solve(A,b,D,d,H,g,initGuess,solver);
+ return solvers::solve(A,b,D,d,H,g,initGuess,d,d,solver);
}
@@ -165,9 +167,13 @@ ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX
}
-ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Dd,const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init, const solvers::SolverType solver)
+ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Dd,
+ const std::pair<MatrixXX, VectorX>& Hg,
+ Cref_vectorX minBounds, Cref_vectorX maxBounds,
+ const VectorX& init,
+ const solvers::SolverType solver)
{
- return solve(Ab.first,Ab.second,Dd.first, Dd.second, Hg.first,Hg.second, init, solver);
+ return solve(Ab.first,Ab.second,Dd.first, Dd.second, Hg.first,Hg.second, init, minBounds, maxBounds, solver);
}
} // namespace bezier_com_traj
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
index 2ecd2bcc9fda46f098dbc6d95d4717c7f919d952..467f182436c4abbd1ef0b9ac2cbc6a6e02301cd3 100644
--- a/src/computeCOMTraj.cpp
+++ b/src/computeCOMTraj.cpp
@@ -304,6 +304,7 @@ ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const doub
{
res.success_ = true;
res.x = resQp.x.head<3>();
+ res.cost_ = resQp.cost_;
std::vector<Vector3> pis = computeConstantWaypoints(pData,T);
res.c_of_t_ = computeBezierCurve<bezier_t, point_t> (pData.constraints_.flag_,T,pis,res.x);
computeFinalVelocity(res);
@@ -372,7 +373,9 @@ long int computeNumEqContinuous(const ProblemData& pData, const int w_degree, co
}
std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pData, const VectorX& Ts,
- std::pair<MatrixXX, VectorX>& Dd){
+ std::pair<MatrixXX, VectorX>& Dd,
+ VectorX& minBounds,
+ VectorX& /*maxBounds*/){
// determine whether to use force or double description
// based on equilibrium value
@@ -395,8 +398,9 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
long int totalVarDim = dimVarX + forceVarDim ;
long int id_rows = 0;
long int id_cols = dimVarX; // start after x variable
- MatrixXX A = MatrixXX::Zero(num_ineq+forceVarDim,totalVarDim);
- VectorX b = VectorX::Zero(num_ineq+forceVarDim);
+ //MatrixXX A = MatrixXX::Zero(num_ineq+forceVarDim,totalVarDim);
+ MatrixXX A = MatrixXX::Zero(num_ineq,totalVarDim);
+ VectorX b = VectorX::Zero(num_ineq);
MatrixXX& D = Dd.first;
VectorX& d = Dd.second;
D = MatrixXX::Zero(num_eq,totalVarDim);
@@ -457,8 +461,11 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
if(!useDD)
{
// add positive constraints on forces
- A.block(id_rows,3,forceVarDim,forceVarDim)=MatrixXX::Identity(forceVarDim,forceVarDim)*(-1);
- id_rows += forceVarDim;
+ //A.block(id_rows,3,forceVarDim,forceVarDim)=MatrixXX::Identity(forceVarDim,forceVarDim)*(-1);
+ //id_rows += forceVarDim;
+ minBounds = VectorX::Zero(A.cols()); // * (-std::numeric_limits<double>::infinity());
+ minBounds.head<3>() = VectorX::Ones(3) * (solvers::UNBOUNDED_DOWN);
+ minBounds.tail(forceVarDim) = VectorX::Zero(forceVarDim) ;
}
assert(id_rows == (A.rows()) && "The inequality constraints matrices were not fully filled.");
assert(id_rows == (b.rows()) && "The inequality constraints matrices were not fully filled.");
@@ -496,18 +503,21 @@ ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
T_time timeArray;
std::pair<MatrixXX, VectorX> Ab;
std::pair<MatrixXX, VectorX> Dd;
+ VectorX minBounds, maxBounds;
if(timeStep > 0 ){ // discretized
assert (pData.representation_ == DOUBLE_DESCRIPTION);
timeArray = computeDiscretizedTime(Ts,timeStep);
Ab = computeConstraintsOneStep(pData,Ts,T,timeArray);
Dd = std::make_pair(MatrixXX::Zero(0,Ab.first.cols()),VectorX::Zero(0));
+ minBounds =VectorX::Zero(0);
+ maxBounds =VectorX::Zero(0);
}else{ // continuous
- Ab = computeConstraintsContinuous(pData,Ts, Dd);
+ Ab = computeConstraintsContinuous(pData,Ts, Dd, minBounds, maxBounds);
timeArray = computeDiscretizedTimeFixed(Ts,7); // FIXME : hardcoded value for discretization for cost function in case of continuous formulation for the constraints
}
std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData,Ts,T,timeArray,Ab.first.cols());
VectorX x = VectorX::Zero(Ab.first.cols());
- ResultData resQp = solve(Ab,Dd,Hg, x, pData.representation_ == FORCE ? solvers::SOLVER_GLPK : solvers::SOLVER_QUADPROG);
+ ResultData resQp = solve(Ab,Dd,Hg, minBounds, maxBounds, x, solver);
#if QHULL
if (resQp.success_) printQHullFile(Ab,resQp.x, "bezier_wp.txt");
#endif
diff --git a/src/glpk-wrapper.cpp b/src/glpk-wrapper.cpp
index 1e5a12132d5b1a390f79cc5d4672222c3dd6be6e..50d698248cce7a832305ec6129069339b2efe8cd 100644
--- a/src/glpk-wrapper.cpp
+++ b/src/glpk-wrapper.cpp
@@ -24,11 +24,27 @@
namespace solvers
{
- int solve(const VectorXd & g0,
+ int getType(const VectorXd & mib, const VectorXd & mab, const int i)
+ {
+ const double& mibV = mib(i);
+ const double& mabV = mab(i);
+ int type = GLP_FR;
+ if(mibV > UNBOUNDED_DOWN && mabV < UNBOUNDED_UP)
+ type = GLP_DB;
+ else if(mibV > UNBOUNDED_DOWN)
+ type = GLP_LO;
+ else if(mabV < UNBOUNDED_UP)
+ type = GLP_UP;
+ return type;
+ }
+
+ int solveglpk(const VectorXd & g0,
const MatrixXd & CE,
const VectorXd & ce0,
const MatrixXd & CI,
const VectorXd & ci0,
+ solvers::Cref_vectorX minBounds,
+ solvers::Cref_vectorX maxBounds,
VectorXd& x,
double& cost)
{
@@ -44,10 +60,8 @@
//where GLP_MAX means maximization
//ROWS
- // TODO SPECIFIC
const int numEqConstraints = (int)(CE.rows());
- // for inequality, x.size()-3 last constraints are not relevant as they are the positivity constraints
- const int numIneqConstraints =(int)(CI.rows() - (x.size() -3));
+ const int numIneqConstraints =(int)(CI.rows());
const int num_constraints_total (numEqConstraints + numIneqConstraints);
glp_add_rows(lp, num_constraints_total);
int idrow = 1;//adds three rows to the problem object
@@ -82,16 +96,12 @@
}
//COLUMNS
- glp_add_cols(lp, xsize); //adds three columns to the problem object
- // TODO SPECIFIC
- for (int i =0; i < 3; ++i, ++idcol )
- {
- glp_set_col_bnds(lp, idcol, GLP_FR, 0.0, 0.0);
- glp_set_obj_coef(lp, idcol, g0(i));
- }
- for (int i =3; i < xsize; ++i, ++idcol )
+ glp_add_cols(lp, xsize);
+ VectorXd miB = minBounds.size() > 0 ? minBounds : VectorXd::Ones(xsize) * UNBOUNDED_DOWN;
+ VectorXd maB = maxBounds.size() > 0 ? maxBounds : VectorXd::Ones(xsize) * UNBOUNDED_UP;
+ for (int i=0; i < xsize; ++i, ++idcol )
{
- glp_set_col_bnds(lp, idcol, GLP_LO, 0.0, 0.0);
+ glp_set_col_bnds(lp, idcol, getType(miB, maB, i), miB(i), maB(i));
glp_set_obj_coef(lp, idcol, g0(i));
}
glp_load_matrix(lp,idConsMat-1,ia,ja,ar);
diff --git a/src/solve_0_step.cpp b/src/solve_0_step.cpp
index b1441863f19e8b5696ed253a93738aca21611755..f695c7d789328369017418cbc95e24577a4cfec7 100644
--- a/src/solve_0_step.cpp
+++ b/src/solve_0_step.cpp
@@ -252,7 +252,7 @@ ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double
if(dimPb > 3)
init.tail(3) = pData.l0_;
// rewriting 0.5 || Dx -d ||^2 as x'Hx + g'x
- ResultData resQp = solve(Ab.first,Ab.second,Hg.first,Hg.second, init);
+ ResultData resQp = solve(Ab,Hg, init);
if(resQp.success_)
{
res.success_ = true;
diff --git a/src/solver-abstract.cpp b/src/solver-abstract.cpp
index 253d1337e0a2a996a05a0a0a89ebf16137913eaa..86b543293dcb89dcb38d4a04c5329569118d7077 100644
--- a/src/solver-abstract.cpp
+++ b/src/solver-abstract.cpp
@@ -40,13 +40,59 @@ typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::SparseVector<double> SpVec;
typedef Eigen::SparseVector<int> SpVeci;
-ResultData solve(const MatrixXd & A,
+namespace
+{
+ void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds, std::pair<MatrixXd,VectorXd>& data)
+ {
+ if(minBounds.size() == 0)
+ return;
+ MatrixXd& res = data.first; VectorXd& resv = data.second;
+ MatrixXd D( res.rows()+ res.cols(), res.cols());
+ VectorXd d(resv.rows()+ res.cols());
+ D.block(0,0, res.rows(), res.cols()) = res;
+ D.block(res.rows(),0, res.cols(), res.cols()) = (-1.) * MatrixXd::Identity(res.cols(), res.cols());
+ d.head(resv.size()) = resv;
+ d.tail(res.cols()) = -minBounds;
+ data.first = D;data.second = d;
+ }
+
+ void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds, std::pair<MatrixXd,VectorXd>& data)
+ {
+ if(maxBounds.size() == 0)
+ return;
+ MatrixXd& res = data.first; VectorXd& resv = data.second;
+ MatrixXd D( res.rows()+ res.cols() -3, res.cols());
+ VectorXd d(resv.rows()+ res.cols());
+ D.block(0,0, res.rows(), res.cols()) = res;
+ D.block(res.rows(),0, res.cols(), res.cols()) = MatrixXd::Identity(res.cols(), res.cols());
+ d.head(resv.size()) = resv;
+ d.tail(res.cols()) = maxBounds;
+ data.first = D;data.second = d;
+ }
+
+ std::pair<MatrixXd,VectorXd> addBoundaryConstraintsQuadProg(solvers::Cref_vectorX minBounds,
+ solvers::Cref_vectorX maxBounds,
+ const MatrixXd & CI,
+ const VectorXd & ci0)
+ {
+ std::pair<MatrixXd,VectorXd> data;
+ data.first = CI;
+ data.second = ci0;
+ addConstraintMinBoundQuadProg(minBounds, data);
+ addConstraintMaxBoundQuadProg(maxBounds, data);
+ return data;
+ }
+}
+
+ResultData solve( const MatrixXd & A,
const VectorXd & b,
const MatrixXd & D,
const VectorXd & d,
const MatrixXd & Hess,
const VectorXd & g,
const VectorXd & initGuess,
+ solvers::Cref_vectorX minBounds,
+ solvers::Cref_vectorX maxBounds,
const SolverType solver)
{
assert (!(is_nan(A)));
@@ -68,17 +114,19 @@ ResultData solve(const MatrixXd & A,
* CI = D; ce0 = -d
*/
case SOLVER_QUADPROG:
- case SOLVER_QUADPROG_SPARSE:
+ //case SOLVER_QUADPROG_SPARSE:
{
- MatrixXd CI = -A;
- //MatrixXd CE = D;
+ std::pair<MatrixXd,VectorXd> CIp = addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
VectorXd ce0 = -d;
tsid::solvers::EiquadprogFast QPsolver = tsid::solvers::EiquadprogFast();
tsid::solvers::EiquadprogFast_status status;
- if(solver == SOLVER_QUADPROG)
- status = QPsolver.solve_quadprog(Hess,g,D,ce0,CI,b,res.x);
- else
- status = QPsolver.solve_quadprog_sparse(Hess.sparseView(),g,D,ce0,CI,b,res.x);
+ //if(solver == SOLVER_QUADPROG)
+ status = QPsolver.solve_quadprog(Hess,g,D,ce0,-CIp.first,CIp.second,res.x);
+ /* else
+ {
+ SpMat Hsp = Hess.sparseView();
+ status = QPsolver.solve_quadprog_sparse(Hsp,g,D,ce0,CI,b,res.x);
+ }*/
res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL );
if(res.success_)
res.cost_ = QPsolver.getObjValue();
@@ -87,7 +135,7 @@ ResultData solve(const MatrixXd & A,
#ifdef USE_GLPK_SOLVER
case SOLVER_GLPK:
{
- res.success_ = (solvers::solve(g,D,d,A,b,res.x,res.cost_) == 0);
+ res.success_ = (solvers::solveglpk(g,D,d,A,b,minBounds, maxBounds,res.x,res.cost_) == 0);
return res;
}
#endif
diff --git a/tests/test-transition.cc b/tests/test-transition.cc
index c43c7e7c4a609dd8a60e7ed4bc7f3a41e2040f70..b198cc0331f0c631d63ff3386b1553ea4c0875a8 100644
--- a/tests/test-transition.cc
+++ b/tests/test-transition.cc
@@ -117,19 +117,31 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
if(continuous)
{
// testing all available solvers
- res = bezier_com_traj::computeCOMTraj(pData,Ts);
+ res = bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG);
if(pData.representation_ == bezier_com_traj::FORCE)
{
- bezier_com_traj::ResultDataCOMTraj res2 =
+ /*bezier_com_traj::ResultDataCOMTraj res2 =
bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG_SPARSE);
BOOST_CHECK(res.success_ == res2.success_);
- BOOST_CHECK(!res.success_ || (res.x.head<3>() - res2.x.head<3>()).norm() < EPSILON);
+ if(res.success_)
+ {
+ std::cout << " x " << res.cost_ << std::endl;
+ std::cout << " x2 " << res2.cost_ << std::endl;
+ discretized_check(pData,Ts,res2,pointsPerPhase,t_total);
+ BOOST_CHECK(!res.success_ || (res.x.head<3>() - res2.x.head<3>()).norm() < EPSILON);
+ }*/
#ifdef USE_GLPK_SOLVER
+ //clock_t s0,e0;
+ //s0 = clock();
bezier_com_traj::ResultDataCOMTraj res3 =
bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_GLPK);
+ //e0 = clock();
+ //std::cout<<"Time to perform full lp : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000.<<" ms "<<std::endl;
BOOST_CHECK(res.success_ == res3.success_);
if(res3.success_)
+ {
discretized_check(pData,Ts,res3,pointsPerPhase,t_total);
+ }
#endif
}
}