diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9c19d3561d7b867944a34dbdd75aab916fbfc9f5..62546e97aa3fdeb8ca128f5aeb45da42cac891ce 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,7 +33,7 @@ SETUP_PROJECT()
string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
-OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" OFF)
+OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
# search for python
FINDPYTHON(2.7 REQUIRED)
diff --git a/include/bezier-com-traj/data.hh b/include/bezier-com-traj/data.hh
index 1b6affde8d6207817abaac350481035ace36ea0e..57360d37e7a5c355c3985b5aee6de688ae1d6a3b 100644
--- a/include/bezier-com-traj/data.hh
+++ b/include/bezier-com-traj/data.hh
@@ -65,49 +65,64 @@ namespace bezier_com_traj
VectorX ang_;
};
+ enum BEZIER_COM_TRAJ_DLLAPI ConstraintFlag{
+ INIT_POS = 0x00001,
+ INIT_VEL = 0x00002,
+ INIT_ACC = 0x00004,
+ END_POS = 0x00008,
+ END_VEL = 0x00010,
+ END_ACC = 0x00020
+ };
+
+ inline ConstraintFlag operator~(ConstraintFlag a)
+ {return static_cast<ConstraintFlag>(~static_cast<const int>(a));}
+
+ inline ConstraintFlag operator|(ConstraintFlag a, ConstraintFlag b)
+ {return static_cast<ConstraintFlag>(static_cast<const int>(a) | static_cast<const int>(b));}
+
+ inline ConstraintFlag operator&(ConstraintFlag a, ConstraintFlag b)
+ {return static_cast<ConstraintFlag>(static_cast<const int>(a) & static_cast<const int>(b));}
+
+ inline ConstraintFlag operator^(ConstraintFlag a, ConstraintFlag b)
+ {return static_cast<ConstraintFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));}
+
+ inline ConstraintFlag& operator|=(ConstraintFlag& a, ConstraintFlag b)
+ {return (ConstraintFlag&)((int&)(a) |= static_cast<const int>(b));}
+
+ inline ConstraintFlag& operator&=(ConstraintFlag& a, ConstraintFlag b)
+ {return (ConstraintFlag&)((int&)(a) &= static_cast<const int>(b));}
+
+ inline ConstraintFlag& operator^=(ConstraintFlag& a, ConstraintFlag b)
+ {return (ConstraintFlag&)((int&)(a) ^= static_cast<const int>(b));}
+
struct BEZIER_COM_TRAJ_DLLAPI Constraints
{
Constraints()
- : c0_(true)
- , dc0_(true)
- , ddc0_(false)
- , ddc1_(false)
- , dc1_(true)
- , c1_(true)
+ : flag_(INIT_POS | INIT_VEL | END_VEL | END_POS)
, constraintAcceleration_(true)
, maxAcceleration_(5.)
,reduce_h_(1e-4) {}
- Constraints(bool c0,bool dc0,bool ddc0,bool ddc1,bool dc1,bool c1)
- : c0_(c0)
- , dc0_(dc0)
- , ddc0_(ddc0)
- , ddc1_(ddc1)
- , dc1_(dc1)
- , c1_(c1)
+ Constraints(ConstraintFlag flag)
+ : flag_(flag)
, constraintAcceleration_(true)
, maxAcceleration_(5.)
,reduce_h_(1e-4)
{
- if(dc0_)
+ /*if(dc0_)
assert(c0_ && "You cannot constraint init velocity if init position is not constrained.");
if(ddc0_)
assert(dc0_ && "You cannot constraint init acceleration if init velocity is not constrained.");
if(dc1_)
assert(c1_ && "You cannot constraint final velocity if final position is not constrained.");
if(ddc1_)
- assert(dc1_ && "You cannot constraint final acceleration if final velocity is not constrained.");
+ assert(dc1_ && "You cannot constraint final acceleration if final velocity is not constrained.");*/
}
~Constraints(){}
- bool c0_;
- bool dc0_;
- bool ddc0_;
- bool ddc1_;
- bool dc1_;
- bool c1_;
+ ConstraintFlag flag_;
bool constraintAcceleration_;
double maxAcceleration_;
double reduce_h_;
@@ -184,7 +199,6 @@ namespace bezier_com_traj
point_t dc1_;
point_t ddc1_;
};
-
} // end namespace bezier_com_traj
#endif
diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index 99ca9f998e4782ae7c758add2197f0b1b803e2e0..5eccb1e7117dbb672fe586e5a91072f7166c2488 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -28,7 +28,7 @@ namespace bezier_com_traj
/**
* @brief solveOnestep Tries to solve the one step problem : Given two or three contact phases, an initial and final com position and velocity,
* try to compute the CoM trajectory (as a Bezier curve) that connect them
- * @param pData problem Data. Should contain only two contact phase.
+ * @param pData problem Data. Should contain only two contact phases.
* @param Ts timelength of each contact phase. Should be the same legnth as pData.contacts
* @param timeStep time step used by the discretization
* @return ResultData a struct containing the resulting trajectory, if success is true.
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
index 8fc92f11d47cd18d50f474277785910517f9c185..602d3222152d8088e0dc78afad021b2eb66cdfb6 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -7,16 +7,17 @@
namespace bezier_com_traj{
namespace c0_dc0_c1{
+
typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
+static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_POS;
-
-bool useThisConstraints(Constraints constraints){
+/*bool useThisConstraints(Constraints constraints){
if(constraints.c0_ && constraints.dc0_ && !constraints.ddc0_ && !constraints.ddc1_ && !constraints.dc1_ && constraints.c1_)
return true;
else
return false;
-}
+}*/
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND final position (DEGREE = 3)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one free waypoint (x)
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
index b8d5e4cc0032563a663b09b39179ae57d9d1f444..16e23a8337b6e22b581a367d696f5db755ec68d8 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -7,16 +7,17 @@
namespace bezier_com_traj{
namespace c0_dc0_dc1_c1{
-
typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
-bool useThisConstraints(Constraints constraints){
+static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_POS | END_VEL;
+
+/*bool useThisConstraints(Constraints constraints){
if(constraints.c0_ && constraints.dc0_ && !constraints.ddc0_ && !constraints.ddc1_ && constraints.dc1_ && constraints.c1_)
return true;
else
return false;
-}
+}*/
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (DEGREE = 4)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one free waypoint (x)
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
index f789a4430b57f4f602f10ef7b55e9522abdfe71e..491a452b8ca4f82747b8a0042259d5028e66c3f5 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -10,13 +10,14 @@ namespace c0_dc0_ddc0_c1{
typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
+static const ConstraintFlag flag =INIT_POS | INIT_VEL | INIT_ACC | END_POS;
-bool useThisConstraints(Constraints constraints){
+/*bool useThisConstraints(Constraints constraints){
if(constraints.c0_ && constraints.dc0_ && constraints.ddc0_ && !constraints.ddc1_ && !constraints.dc1_ && constraints.c1_)
return true;
else
return false;
-}
+}*/
/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration, and only final position (degree = 4)
/**
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
index 8a3d8ddac53640eb773ee23134ed6a1f8df351ee..782060dc3c195f384350c898c49538906165a2bc 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -8,16 +8,17 @@
namespace bezier_com_traj{
namespace c0_dc0_ddc0_dc1_c1{
+
typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_VEL | END_POS;
-
-bool useThisConstraints(Constraints constraints){
+/*bool useThisConstraints(Constraints constraints){
if(constraints.c0_ && constraints.dc0_ && constraints.ddc0_ && !constraints.ddc1_ && constraints.dc1_ && constraints.c1_)
return true;
else
return false;
-}
+}*/
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND INIT ACCELERATION (DEGREE = 5)
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
index 0e5ebe1ead5f813dbdd502d3c691945c7b38c8fa..9613b8ba0ea2449cbf45aea3b34004f1e61ffee2 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -11,12 +11,15 @@ namespace c0_dc0_ddc0_ddc1_dc1_c1{
typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
-bool useThisConstraints(Constraints constraints){
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
+
+/*bool useThisConstraints(Constraints constraints){
if(constraints.c0_ && constraints.dc0_ && constraints.ddc0_ && constraints.ddc1_ && constraints.dc1_ && constraints.c1_)
return true;
else
return false;
-}
+}*/
+
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION (DEGREE = 6)
///
/**
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
index c9a2bf9f697a6dc7f39e8a81f67c5aad370722d6..21c57a5816c0ecd065f959c285db161492472138 100644
--- a/include/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -8,109 +8,159 @@
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh>
+#include "boost/assign.hpp"
namespace bezier_com_traj{
-typedef waypoint3_t waypoint_t;
-typedef std::pair<double,point3_t> coefs_t;
+
/**
* This file is used to choose the correct expressions of the curves waypoints, depending on the options set in ProblemData.constraints
*/
+typedef std::pair<double,point3_t> coefs_t;
+typedef coefs_t (*evalCurveAtTime) (std::vector<point_t> pi,double t);
+typedef std::map<ConstraintFlag,evalCurveAtTime > T_evalCurveAtTime;
+typedef T_evalCurveAtTime::const_iterator CIT_evalCurveAtTime;
+static const T_evalCurveAtTime evalCurveAtTimes = boost::assign::map_list_of
+ (c0_dc0_c1::flag , c0_dc0_c1::evaluateCurveAtTime)
+ (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::evaluateCurveAtTime)
+ (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::evaluateCurveAtTime)
+ (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime);
+
+
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateCurveAtTime(const ProblemData& pData, std::vector<point_t> pi,double t){
- if(c0_dc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_c1::evaluateCurveAtTime(pi,t);
- else if(c0_dc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_dc1_c1::evaluateCurveAtTime(pi,t);
- else if(c0_dc0_ddc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_c1::evaluateCurveAtTime(pi,t);
- else if(c0_dc0_ddc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime(pi,t);
- else if(c0_dc0_ddc0_ddc1_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime(pi,t);
- else{
+coefs_t evaluateCurveAtTime(const ProblemData& pData, std::vector<point_t> pi,double t)
+{
+ CIT_evalCurveAtTime cit = evalCurveAtTimes.find(pData.constraints_.flag_);
+ if(cit != evalCurveAtTimes.end())
+ return cit->second(pi,t);
+ else
+ {
std::cout<<"Current constraints set are not implemented"<<std::endl;
throw std::runtime_error("Current constraints set are not implemented");
}
}
+
+
+typedef coefs_t (*evalAccCurveAtTime) (std::vector<point_t> pi,double T,double t);
+typedef std::map<ConstraintFlag,evalAccCurveAtTime > T_evalAccCurveAtTime;
+typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
+static const T_evalAccCurveAtTime evalAccCurveAtTimes = boost::assign::map_list_of
+ (c0_dc0_c1::flag , c0_dc0_c1::evaluateAccelerationCurveAtTime)
+ (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)
+ (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime)
+ (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
+
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
*/
-coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, std::vector<point_t> pi,double T,double t){
- if(c0_dc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_c1::evaluateAccelerationCurveAtTime(pi,T,t);
- else if(c0_dc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime(pi,T,t);
- else if(c0_dc0_ddc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime(pi,T,t);
- else if(c0_dc0_ddc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime(pi,T,t);
- else if(c0_dc0_ddc0_ddc1_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime(pi,T,t);
- else{
+coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, std::vector<point_t> pi,double T,double t)
+{
+ CIT_evalAccCurveAtTime cit = evalAccCurveAtTimes.find(pData.constraints_.flag_);
+ if(cit != evalAccCurveAtTimes.end())
+ return cit->second(pi,T,t);
+ else
+ {
std::cout<<"Current constraints set are not implemented"<<std::endl;
throw std::runtime_error("Current constraints set are not implemented");
}
}
+
+
+typedef std::vector<point_t> (*compConsWp) (const ProblemData& pData,double T);
+typedef std::map<ConstraintFlag,compConsWp > T_compConsWp;
+typedef T_compConsWp::const_iterator CIT_compConsWp;
+static const T_compConsWp compConsWps = boost::assign::map_list_of
+ (c0_dc0_c1::flag , c0_dc0_c1::computeConstantWaypoints)
+ (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeConstantWaypoints)
+ (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeConstantWaypoints)
+ (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
- if(c0_dc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_c1::computeConstantWaypoints(pData,T);
- else if(c0_dc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_dc1_c1::computeConstantWaypoints(pData,T);
- else if(c0_dc0_ddc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_c1::computeConstantWaypoints(pData,T);
- else if(c0_dc0_ddc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_dc1_c1::computeConstantWaypoints(pData,T);
- else if(c0_dc0_ddc0_ddc1_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints(pData,T);
- else{
+std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
+{
+ CIT_compConsWp cit = compConsWps.find(pData.constraints_.flag_);
+ if(cit != compConsWps.end())
+ return cit->second(pData,T);
+ else
+ {
std::cout<<"Current constraints set are not implemented"<<std::endl;
throw std::runtime_error("Current constraints set are not implemented");
}
}
+
+typedef std::vector<waypoint6_t> (*compWp) (const ProblemData& pData,double T);
+typedef std::map<ConstraintFlag,compWp > T_compWp;
+typedef T_compWp::const_iterator CIT_compWp;
+static const T_compWp compWps = boost::assign::map_list_of
+ (c0_dc0_c1::flag , c0_dc0_c1::computeWwaypoints)
+ (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeWwaypoints)
+ (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeWwaypoints)
+ (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeWwaypoints)
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints);
+
/**
* @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states
* @param pData
* @param T
* @return
*/
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- if(c0_dc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_c1::computeWwaypoints(pData,T);
- else if(c0_dc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_dc1_c1::computeWwaypoints(pData,T);
- else if(c0_dc0_ddc0_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_c1::computeWwaypoints(pData,T);
- else if(c0_dc0_ddc0_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_dc1_c1::computeWwaypoints(pData,T);
- else if(c0_dc0_ddc0_ddc1_dc1_c1::useThisConstraints(pData.constraints_))
- return c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints(pData,T);
- else{
+std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T)
+{
+ CIT_compWp cit = compWps.find(pData.constraints_.flag_);
+ if(cit != compWps.end())
+ return cit->second(pData,T);
+ else
+ {
std::cout<<"Current constraints set are not implemented"<<std::endl;
throw std::runtime_error("Current constraints set are not implemented");
}
}
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
+typedef coefs_t (*compFinalAccP) (const ProblemData& pData,double T);
+typedef std::map<ConstraintFlag,compFinalAccP > T_compFinalAccP;
+typedef T_compFinalAccP::const_iterator CIT_compFinalAccP;
+static const T_compFinalAccP compFinalAccPs = boost::assign::map_list_of
+ (c0_dc0_c1::flag , c0_dc0_c1::computeFinalAccelerationPoint)
+ (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeFinalAccelerationPoint)
+ (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeFinalAccelerationPoint)
+ (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeFinalAccelerationPoint)
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeFinalAccelerationPoint);
+
+coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T)
+{
+ CIT_compFinalAccP cit = compFinalAccPs.find(pData.constraints_.flag_);
+ if(cit != compFinalAccPs.end())
+ return cit->second(pData,T);
+ else
+ {
+ std::cout<<"Current constraints set are not implemented"<<std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
+}
+
+
+
+/*coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
if(c0_dc0_c1::useThisConstraints(pData.constraints_))
return c0_dc0_c1::computeFinalAccelerationPoint(pData,T);
else if(c0_dc0_dc1_c1::useThisConstraints(pData.constraints_))
@@ -134,10 +184,43 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
std::cout<<"Current constraints set are not implemented"<<std::endl;
throw std::runtime_error("Current constraints set are not implemented");
}
+}*/
+
+
+typedef coefs_t (*compFinalVelP) (const ProblemData& pData,double T);
+typedef std::map<ConstraintFlag,compFinalVelP > T_compFinalVelP;
+typedef T_compFinalVelP::const_iterator CIT_compFinalVelP;
+static const T_compFinalVelP compFinalVelPs = boost::assign::map_list_of
+ (c0_dc0_c1::flag , c0_dc0_c1::computeFinalVelocityPoint)
+ (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeFinalVelocityPoint)
+ (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeFinalVelocityPoint)
+ (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
+
+coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T)
+{
+ CIT_compFinalVelP cit = compFinalVelPs.find(pData.constraints_.flag_);
+ if(cit != compFinalVelPs.end())
+ return cit->second(pData,T);
+ else
+ {
+ std::cout<<"Current constraints set are not implemented"<<std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
}
+/*coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+ if(c0_dc0_c1::useThisConstraints(pData.constraints_))
+ return c0_dc0_c1::computeFinalVelocityPoint(pData,T);
+ else if(c0_dc0_ddc0_c1::useThisConstraints(pData.constraints_))
+ return c0_dc0_ddc0_c1::computeFinalVelocityPoint(pData,T);
+ else{
+ std::cout<<"Current constraints set are not implemented"<<std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
+}*/
void computeFinalAcceleration(const ProblemData& pData,double T,ResultDataCOMTraj& res){
- if(pData.constraints_.ddc1_){
+ if(pData.constraints_.flag_& END_ACC){
res.ddc1_ = pData.ddc1_;
}else{
coefs_t a = computeFinalAccelerationPoint(pData,T);
@@ -146,7 +229,7 @@ void computeFinalAcceleration(const ProblemData& pData,double T,ResultDataCOMTra
}
void computeFinalVelocity(const ProblemData& pData,double T,ResultDataCOMTraj& res){
- if(pData.constraints_.dc1_)
+ if(pData.constraints_.flag_& END_VEL)
res.dc1_ = pData.dc1_;
else{
coefs_t v = computeFinalVelocityPoint(pData,T);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f245f80faad28868ca8f2368b8ecc1206a31f44a..2d113aa60d628ad54f2e706170663d068250a955 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -13,6 +13,13 @@ SET(${LIBRARY_NAME}_SOURCES
${INCLUDE_DIR}/bezier-com-traj/data.hh
${INCLUDE_DIR}/bezier-com-traj/solve.hh
${INCLUDE_DIR}/bezier-com-traj/common_solve_methods.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_definition.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
${INCLUDE_DIR}/solver/eiquadprog-fast.hpp
common_solve_methods.cpp
eiquadprog-fast.cpp
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index c118a590b0e419020b7edb012781777ead093031..e36d04ed59676a666a3da8566f9861d77a7260d4 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -407,13 +407,13 @@ void computeBezierCurve(const ProblemData& pData, const double T, ResultDataCOMT
std::vector<Vector3> pi = computeConstantWaypoints(pData,T);
size_t i = 0;
- if(pData.constraints_.c0_){
+ if(pData.constraints_.flag_ & INIT_POS ){
wps.push_back(pi[i]);
i++;
- if(pData.constraints_.dc0_){
+ if(pData.constraints_.flag_ & INIT_VEL){
wps.push_back(pi[i]);
i++;
- if(pData.constraints_.ddc0_){
+ if(pData.constraints_.flag_ & INIT_ACC){
wps.push_back(pi[i]);
i++;
}
@@ -421,17 +421,17 @@ void computeBezierCurve(const ProblemData& pData, const double T, ResultDataCOMT
}
wps.push_back(res.x);
i++;
- if(pData.constraints_.ddc1_){
- assert(pData.constraints_.dc1_ && "You cannot constraint final acceleration if final velocity is not constrained.");
+ 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_.dc1_){
- assert(pData.constraints_.c1_ && "You cannot constraint final velocity if final position is not constrained.");
+ 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_.c1_){
+ if(pData.constraints_.flag_ & END_POS){
wps.push_back(pi[i]);
i++;
}
@@ -476,7 +476,7 @@ ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const
VectorX constraint_equivalence;
std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,pointsPerPhase,constraint_equivalence);
//computeCostMidPoint(pData,H,g);
- if(pData.constraints_.dc1_)
+ if(pData.constraints_.flag_ & END_VEL)
computeCostMinAcceleration(pData,Ts,pointsPerPhase,H,g);
else
computeCostEndVelocity(pData,T,H,g);
diff --git a/tests/test-transition.cc b/tests/test-transition.cc
index e5456fedd3e51450451febc72f4f88165bb1259f..d4ca1c2ff8f6d5f2312f0098646de2e799dfbb67 100644
--- a/tests/test-transition.cc
+++ b/tests/test-transition.cc
@@ -21,6 +21,7 @@
#include <boost/test/included/unit_test.hpp>
#include <bezier-com-traj/solve.hh>
#include <bezier-com-traj/common_solve_methods.hh>
+#include <bezier-com-traj/data.hh>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
#include <test_helper.hh>
@@ -232,7 +233,7 @@ BOOST_AUTO_TEST_CASE(transition){
BOOST_AUTO_TEST_CASE(transition_noDc1){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.dc1_ = false;
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
check_transition(pData,Ts);
@@ -240,7 +241,7 @@ BOOST_AUTO_TEST_CASE(transition_noDc1){
BOOST_AUTO_TEST_CASE(transition_ddc0){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
check_transition(pData,Ts);
@@ -248,8 +249,8 @@ BOOST_AUTO_TEST_CASE(transition_ddc0){
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
- pData.constraints_.ddc1_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
+ pData.constraints_.flag_ |= bezier_com_traj::END_ACC;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
check_transition(pData,Ts);
@@ -265,7 +266,7 @@ BOOST_AUTO_TEST_CASE(transition_noAcc){
BOOST_AUTO_TEST_CASE(transition_noDc1_noAcc){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.dc1_ = false;
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
pData.constraints_.constraintAcceleration_ = false;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -273,7 +274,7 @@ BOOST_AUTO_TEST_CASE(transition_noDc1_noAcc){
}
BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
pData.constraints_.constraintAcceleration_ = false;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -282,8 +283,7 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc){
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_noAcc){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
- pData.constraints_.ddc1_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
pData.constraints_.constraintAcceleration_ = false;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -301,7 +301,7 @@ BOOST_AUTO_TEST_CASE(transition_Acc1){
BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.dc1_ = false;
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
pData.constraints_.maxAcceleration_=1.;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -309,7 +309,7 @@ BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1){
}
BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
pData.constraints_.maxAcceleration_=2.;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -318,8 +318,7 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2){
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
- pData.constraints_.ddc1_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
pData.constraints_.maxAcceleration_=2.;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -328,8 +327,7 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2){
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc05){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
- pData.constraints_.ddc1_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
pData.constraints_.maxAcceleration_=0.5;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -356,7 +354,7 @@ BOOST_AUTO_TEST_CASE(transition_Acc02){
BOOST_AUTO_TEST_CASE(transition_noDc1_Acc05){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.dc1_ = false;
+ pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
pData.constraints_.maxAcceleration_=0.5;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -366,7 +364,7 @@ BOOST_AUTO_TEST_CASE(transition_noDc1_Acc05){
BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
pData.constraints_.maxAcceleration_=1.;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
@@ -375,8 +373,7 @@ BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1){
BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc02){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
- pData.constraints_.ddc0_ = true;
- pData.constraints_.ddc1_ = true;
+ pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
pData.constraints_.maxAcceleration_=0.2;
VectorX Ts(3);
Ts<<0.6,0.6,0.6;