diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff96906fdd4b52694c1ecba6e176eaa31c190f4a..f2a41e50f0bb13f4202f82e205f2996519b3e5b0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,7 @@ find_package (Spline REQUIRED)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/bezier-com-traj/data.hh
+ include/bezier-com-traj/utils.hh
include/bezier-com-traj/flags.hh
include/bezier-com-traj/definitions.hh
include/bezier-com-traj/config.hh
diff --git a/include/bezier-com-traj/common_solve_methods.hh b/include/bezier-com-traj/common_solve_methods.hh
index 52e41a4ed94031e5bfabde2c9ca205751678bb42..1e7aa0c0c0e2487b42f88272f14e8ee1db246c34 100644
--- a/include/bezier-com-traj/common_solve_methods.hh
+++ b/include/bezier-com-traj/common_solve_methods.hh
@@ -13,12 +13,7 @@
namespace bezier_com_traj
{
-
-
-BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
template<typename T> T initwp();
-int Normalize(Ref_matrixXX A, Ref_vectorX b);
-
/**
* @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
@@ -42,7 +37,6 @@ BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(con
*/
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL, const bool useAngMomentum, bool& fail);
-
/**
* @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog
* @param A Inequality matrix
@@ -62,13 +56,6 @@ BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_ma
*/
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg, const Vector3& init);
-/**
- * @brief Compute the Bernstein polynoms for a given degree
- * @param degree required degree
- * @return
- */
-std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
-
} // end namespace bezier_com_traj
#endif
diff --git a/include/bezier-com-traj/data.hh b/include/bezier-com-traj/data.hh
index 0854d9c9cfed6539ded9a7b3101019e47ee4e4dc..d9c2bfcd8956cd798a1b0cb3f42e53aa03a9bfdc 100644
--- a/include/bezier-com-traj/data.hh
+++ b/include/bezier-com-traj/data.hh
@@ -9,6 +9,7 @@
#include <bezier-com-traj/config.hh>
#include <bezier-com-traj/flags.hh>
#include <bezier-com-traj/definitions.hh>
+#include <bezier-com-traj/utils.hh>
#include <spline/bezier_curve.h>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
@@ -80,18 +81,18 @@ namespace bezier_com_traj
struct BEZIER_COM_TRAJ_DLLAPI ProblemData
{
ProblemData()
- : c0_ (Vector3::Zero())
- ,dc0_ (Vector3::Zero())
- ,ddc0_(Vector3::Zero())
- , c1_ (Vector3::Zero())
- ,dc1_ (Vector3::Zero())
- ,ddc1_(Vector3::Zero())
+ : c0_ (point_t::Zero())
+ ,dc0_ (point_t::Zero())
+ ,ddc0_(point_t::Zero())
+ , c1_ (point_t::Zero())
+ ,dc1_ (point_t::Zero())
+ ,ddc1_(point_t::Zero())
,useAngularMomentum_(false)
,costFunction_(ACCELERATION) {}
std::vector<ContactData> contacts_;
- Vector3 c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
- Vector3 l0_;
+ point_t c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
+ point_t l0_;
bool useAngularMomentum_;
Constraints constraints_;
CostFunction costFunction_;
diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index cf5a98d9b2a456e621d7b56f9cc077b535aaa65f..8c74c51b5d4dff50d4cd548d8e70c1fa38261fb2 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -35,12 +35,6 @@ namespace bezier_com_traj
*/
BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts, const Vector3& init_guess,const int pointsPerPhase = 3, const double feasability_treshold = 0.);
-
- void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,const std::string& fileName,bool clipZ = false);
-
-
-
-
} // end namespace bezier_com_traj
#endif
diff --git a/include/bezier-com-traj/utils.hh b/include/bezier-com-traj/utils.hh
new file mode 100644
index 0000000000000000000000000000000000000000..b7412fc635bfb4a05e7f3f9040e169404120e4fd
--- /dev/null
+++ b/include/bezier-com-traj/utils.hh
@@ -0,0 +1,98 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Steve Tonneau
+ */
+
+#ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
+#define BEZIER_COM_TRAJ_LIB_UTILS_H
+
+#include <bezier-com-traj/config.hh>
+#include <bezier-com-traj/definitions.hh>
+#include <bezier-com-traj/flags.hh>
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+namespace bezier_com_traj
+{
+
+/**
+ * @brief Compute the Bernstein polynoms for a given degree
+ * @param degree required degree
+ * @return the bernstein polynoms
+ */
+BEZIER_COM_TRAJ_DLLAPI std::vector<spline::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
+
+
+/**
+ * @brief given the constraints of the problem, and a set of waypoints, return
+ * the bezier curve corresponding
+ * @param pData problem data
+ * @param T total trajectory time
+ * @param pis list of waypoints
+ * @return the bezier curve
+ */
+template<typename Point>
+BEZIER_COM_TRAJ_DLLAPI bezier_t computeBezierCurve(const ConstraintFlag& flag, const double T,
+ const std::vector<Point>& pi, const Point& x);
+
+/**
+ * @brief write a polytope describe by A x <= b linear constraints in
+ * a given filename
+ * @return the bernstein polynoms
+ */
+void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,
+ const std::string& fileName,bool clipZ = false);
+
+/**
+ * @brief skew symmetric matrix
+ */
+BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
+
+/**
+ * @brief normalize inequality constraints
+ */
+int Normalize(Ref_matrixXX A, Ref_vectorX b);
+
+} // end namespace bezier_com_traj
+
+template<typename Point>
+bezier_com_traj::bezier_t bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const double T,
+ const std::vector<Point>& pi, const Point& x)
+{
+ std::vector<Point> wps;
+ size_t i = 0;
+ if(flag & INIT_POS ){
+ wps.push_back(pi[i]);
+ i++;
+ if(flag & INIT_VEL){
+ wps.push_back(pi[i]);
+ i++;
+ if(flag & INIT_ACC){
+ wps.push_back(pi[i]);
+ i++;
+ }
+ }
+ }
+ wps.push_back(x);
+ i++;
+ if(flag & END_ACC){
+ assert(pData.constraints_.flag_ & END_VEL && "You cannot constrain final acceleration if final velocity is not constrained.");
+ wps.push_back(pi[i]);
+ i++;
+ }
+ if(flag & END_VEL){
+ assert(pData.constraints_.flag_ & END_POS && "You cannot constrain final velocity if final position is not constrained.");
+ wps.push_back(pi[i]);
+ i++;
+ }
+ if(flag & END_POS){
+ wps.push_back(pi[i]);
+ i++;
+ }
+ return bezier_t (wps.begin(), wps.end(),T);
+}
+
+
+#endif
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index d0de820727a25f8c055a9e079a0dd571ba8a2935..bf10c17ba7f2b086920434a2a98b069dd8c66118 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -245,7 +245,6 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
// TODO : filter for redunbdant constraints ...
// add stability constraints :
Ab_stab = dynamicStabilityConstraints(mH,h,g,wps_w[id_step]);
- //compareStabilityMethods(mH,h,g,wps_c[id_step],wps_ddc[id_step],wps_w[id_step]);
A.block(id_rows,0,dimH,numCol) = Ab_stab.first;
b.segment(id_rows,dimH) = Ab_stab.second;
id_rows += dimH ;
@@ -334,45 +333,6 @@ void computeCostEndVelocity(const ProblemData& pData,const double T, MatrixXX& H
g.head<3> () = v.first*(v.second - pData.dc1_);
}
-
-void computeBezierCurve(const ProblemData& pData, const double T, ResultDataCOMTraj& res)
-{
- std::vector<Vector3> wps;
-
- std::vector<Vector3> pi = computeConstantWaypoints(pData,T);
- size_t i = 0;
- if(pData.constraints_.flag_ & INIT_POS ){
- wps.push_back(pi[i]);
- i++;
- if(pData.constraints_.flag_ & INIT_VEL){
- wps.push_back(pi[i]);
- i++;
- if(pData.constraints_.flag_ & INIT_ACC){
- wps.push_back(pi[i]);
- i++;
- }
- }
- }
- wps.push_back(res.x);
- i++;
- if(pData.constraints_.flag_ & END_ACC){
- assert(pData.constraints_.flag_ & END_VEL && "You cannot constraint final acceleration if final velocity is not constrained.");
- wps.push_back(pi[i]);
- i++;
- }
- if(pData.constraints_.flag_ & END_VEL){
- assert(pData.constraints_.flag_ & END_POS && "You cannot constraint final velocity if final position is not constrained.");
- wps.push_back(pi[i]);
- i++;
- }
- if(pData.constraints_.flag_ & END_POS){
- wps.push_back(pi[i]);
- i++;
- }
-
- res.c_of_t_ = bezier_t (wps.begin(), wps.end(),T);
-}
-
void computeFinalAcceleration(ResultDataCOMTraj& res){
res.ddc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 2);
}
@@ -405,7 +365,8 @@ ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const doub
{
res.success_ = true;
res.x = resQp.x.head<3>();
- computeBezierCurve (pData,T,res);
+ std::vector<Vector3> pis = computeConstantWaypoints(pData,T);
+ res.c_of_t_ = computeBezierCurve<point_t> (pData.constraints_.flag_,T,pis,res.x);
computeFinalVelocity(res);
computeFinalAcceleration(res);
}