diff --git a/include/bezier-com-traj/data.hh b/include/bezier-com-traj/data.hh
index d9c2bfcd8956cd798a1b0cb3f42e53aa03a9bfdc..8dca5119feb1ea12a79b67e49493ebbfcb36b733 100644
--- a/include/bezier-com-traj/data.hh
+++ b/include/bezier-com-traj/data.hh
@@ -84,14 +84,16 @@ namespace bezier_com_traj
: c0_ (point_t::Zero())
,dc0_ (point_t::Zero())
,ddc0_(point_t::Zero())
+ ,j0_(point_t::Zero())
, c1_ (point_t::Zero())
,dc1_ (point_t::Zero())
,ddc1_(point_t::Zero())
+ ,j1_(point_t::Zero())
,useAngularMomentum_(false)
,costFunction_(ACCELERATION) {}
std::vector<ContactData> contacts_;
- point_t c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
+ point_t c0_,dc0_,ddc0_,j0_,c1_,dc1_,ddc1_,j1_;
point_t l0_;
bool useAngularMomentum_;
Constraints constraints_;
@@ -146,7 +148,6 @@ namespace bezier_com_traj
, dL_of_t_(bezier_t::zero())
, dc1_(point_t::Zero())
, ddc1_(point_t::Zero()) {}
-
~ResultDataCOMTraj(){}
bezier_t c_of_t_; // center of mass trajectory
diff --git a/include/bezier-com-traj/flags.hh b/include/bezier-com-traj/flags.hh
index 995085afe3d7f207582678113cb8d2f267004d1a..ab3bbae8afce5b49a29da1d4ffcde2a797e79c93 100644
--- a/include/bezier-com-traj/flags.hh
+++ b/include/bezier-com-traj/flags.hh
@@ -18,13 +18,15 @@ namespace bezier_com_traj
};
enum BEZIER_COM_TRAJ_DLLAPI ConstraintFlag{
- INIT_POS = 0x00001,
- INIT_VEL = 0x00002,
- INIT_ACC = 0x00004,
- END_POS = 0x00008,
- END_VEL = 0x00010,
- END_ACC = 0x00020,
- UNKNOWN = 0x00040
+ INIT_POS = 0x00001,
+ INIT_VEL = 0x00002,
+ INIT_ACC = 0x00004,
+ END_POS = 0x00008,
+ END_VEL = 0x00010,
+ END_ACC = 0x00020,
+ INIT_JERK = 0x00040,
+ END_JERK = 0x00080,
+ UNKNOWN = 0x00100
};
inline ConstraintFlag operator~(ConstraintFlag a)
diff --git a/include/bezier-com-traj/solve_end_effector.hh b/include/bezier-com-traj/solve_end_effector.hh
index ef0604eacaa838e2ae4f9723064f4a4d078152d7..14b7ad351f349f05b52ac210d45abeb82bb01042 100644
--- a/include/bezier-com-traj/solve_end_effector.hh
+++ b/include/bezier-com-traj/solve_end_effector.hh
@@ -38,33 +38,192 @@ coefs_t initCoefs(){
return c;
}
+
+void computeConstantWaypoints(const ProblemData& pData,double T,double n,point_t& p0,point_t& p1, point_t& p2, point_t& p4, point_t& p5, point_t& p6){
+ p0 = pData.c0_;
+ p1 = (pData.dc0_ * T / n )+ pData.c0_;
+ p2 = (pData.ddc0_*T*T/(n*(n-1))) + (2*pData.dc0_ *T / n) + pData.c0_; // * T because derivation make a T appear
+ p6 = pData.c1_;
+ p5 = (-pData.dc1_ * T / n) + pData.c1_; // * T ?
+ p4 = (pData.ddc1_ *T*T / (n*(n-1))) - (2 * pData.dc1_ *T / n) + pData.c1_ ; // * T ??
+}
+
void computeConstantWaypoints(const ProblemData& pData,double T,double n,point_t& p0,point_t& p1, point_t& p2, point_t& p3, point_t& p5, point_t& p6,point_t& p7,point_t& p8){
p0 = pData.c0_;
p1 = (pData.dc0_ * T / n )+ pData.c0_;
p2 = (pData.ddc0_*T*T/(n*(n-1))) + (2*pData.dc0_ *T / n) + pData.c0_; // * T because derivation make a T appear
- p3 = (3*pData.ddc0_*T*T/(n*(n-1))) + (3*pData.dc0_ *T / n) + pData.c0_;
+ p3 = (pData.j0_*T*T*T/(n*(n-1)*(n-2)))+ (3*pData.ddc0_*T*T/(n*(n-1))) + (3*pData.dc0_ *T / n) + pData.c0_;
+
p8 = pData.c1_;
p7 = (-pData.dc1_ * T / n) + pData.c1_; // * T ?
p6 = (pData.ddc1_ *T*T / (n*(n-1))) - (2 * pData.dc1_ *T / n) + pData.c1_ ; // * T ??
- p5 = (3*pData.ddc1_ *T*T / (n*(n-1))) - (3 * pData.dc1_ *T / n) + pData.c1_ ; // * T ??
+ p5 = (-pData.j1_*T*T*T/(n*(n-1)*(n-2))) + (3*pData.ddc1_ *T*T / (n*(n-1))) - (3 * pData.dc1_ *T / n) + pData.c1_ ; // * T ??
+}
+
+// with jerk and jerk derivative constrained to 0
+void computeConstantWaypoints(const ProblemData& pData,double T,double n,point_t& p0,point_t& p1, point_t& p2, point_t& p3, point_t& p4, point_t& p5,point_t& p6,point_t& p7,point_t& p8,point_t& p9){
+ p0 = pData.c0_;
+ p1 = (pData.dc0_ * T / n )+ pData.c0_;
+ p2 = (n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T - 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1)); // * T because derivation make a T appear
+ p3 = (n*n*pData.c0_ - n*pData.c0_ + 3*n*pData.dc0_*T - 3*pData.dc0_*T + 3*pData.ddc0_*T*T)/(n*(n - 1));
+ p4 = (n*n*pData.c0_ - n*pData.c0_ + 4*n*pData.dc0_*T - 4*pData.dc0_ *T+ 6*pData.ddc0_*T*T)/(n*(n - 1)) ;
+
+ p9 = pData.c1_;
+ p8 = (-pData.dc1_ * T / n) + pData.c1_; // * T ?
+ p7 = (n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T + 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1)) ; // * T ??
+ p6 = (n*n*pData.c1_ - n*pData.c1_ - 3*n*pData.dc1_*T + 3*pData.dc1_*T + 3*pData.ddc1_*T*T)/(n*(n - 1)) ; // * T ??
+ p5 = (n*n*pData.c1_ - n*pData.c1_ - 4*n*pData.dc1_*T + 4*pData.dc1_*T + 6*pData.ddc1_*T*T)/(n*(n - 1)) ;
+}
+
+
+// with jerk and jerk second derivative constrained to 0
+void computeConstantWaypoints(const ProblemData& pData,double T,double n,point_t& p0,point_t& p1, point_t& p2, point_t& p3, point_t& p4, point_t& p5,point_t& p6,point_t& p7,point_t& p8,point_t& p9,point_t& p10,point_t& p11){
+ p0 = pData.c0_;
+ p1 = (pData.dc0_ * T / n )+ pData.c0_;
+ p2 = (n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T - 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1)); // * T because derivation make a T appear
+ p3 = (n*n*pData.c0_ - n*pData.c0_ + 3*n*pData.dc0_*T - 3*pData.dc0_*T + 3*pData.ddc0_*T*T)/(n*(n - 1));
+ p4 = (n*n*pData.c0_ - n*pData.c0_ + 4*n*pData.dc0_*T - 4*pData.dc0_ *T+ 6*pData.ddc0_*T*T)/(n*(n - 1)) ;
+ p5 = (n*n*pData.c0_ - n*pData.c0_ + 5*n*pData.dc0_*T - 5*pData.dc0_ *T+ 10*pData.ddc0_*T*T)/(n*(n - 1)) ;
+
+ p11 = pData.c1_;
+ p10 = (-pData.dc1_ * T / n) + pData.c1_; // * T ?
+ p9 = (n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T + 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1)) ; // * T ??
+ p8 = (n*n*pData.c1_ - n*pData.c1_ - 3*n*pData.dc1_*T + 3*pData.dc1_*T + 3*pData.ddc1_*T*T)/(n*(n - 1)) ; // * T ??
+ p7 = (n*n*pData.c1_ - n*pData.c1_ - 4*n*pData.dc1_*T + 4*pData.dc1_*T + 6*pData.ddc1_*T*T)/(n*(n - 1)) ;
+ p6 = (n*n*pData.c1_ - n*pData.c1_ - 5*n*pData.dc1_*T + 5*pData.dc1_*T + 10*pData.ddc1_*T*T)/(n*(n - 1)) ;
+}
+
+// up to jerk second derivativ constraints for init, pos vel and acc constraint for goal
+std::vector<bezier_t::point_t> computeConstantWaypointsInitPredef(const ProblemData& pData,double T){
+ const double n = 8;
+ std::vector<bezier_t::point_t> pts;
+ pts.push_back(pData.c0_);
+ pts.push_back((pData.dc0_ * T / n )+ pData.c0_);
+ pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T - 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1))); // * T because derivation make a T appear
+ pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 3*n*pData.dc0_*T - 3*pData.dc0_*T + 3*pData.ddc0_*T*T)/(n*(n - 1)));
+ pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 4*n*pData.dc0_*T - 4*pData.dc0_ *T+ 6*pData.ddc0_*T*T)/(n*(n - 1))) ;
+ pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 5*n*pData.dc0_*T - 5*pData.dc0_ *T+ 10*pData.ddc0_*T*T)/(n*(n - 1))) ;
+
+ pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T + 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1))) ; // * T ??
+ pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
+ pts.push_back(pData.c1_);
+
+ return pts;
+}
+
+
+// up to jerk second derivativ constraints for goal, pos vel and acc constraint for init
+std::vector<bezier_t::point_t> computeConstantWaypointsGoalPredef(const ProblemData& pData,double T){
+ const double n = 8;
+ std::vector<bezier_t::point_t> pts;
+ pts.push_back(pData.c0_);
+ pts.push_back((pData.dc0_ * T / n )+ pData.c0_);
+ pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T - 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1))); // * T because derivation make a T appear
+
+ pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 5*n*pData.dc1_*T + 5*pData.dc1_*T + 10*pData.ddc1_*T*T)/(n*(n - 1))) ;
+ pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 4*n*pData.dc1_*T + 4*pData.dc1_*T + 6*pData.ddc1_*T*T)/(n*(n - 1))) ;
+ pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 3*n*pData.dc1_*T + 3*pData.dc1_*T + 3*pData.ddc1_*T*T)/(n*(n - 1))) ; // * T ??
+ pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T + 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1))) ; // * T ??
+ pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
+ pts.push_back(pData.c1_);
+
+ return pts;
}
std::vector<bezier_t::point_t> computeConstantWaypoints(const ProblemData& pData,double T,double n){
- point_t p0,p1,p2,p3,p4,p5,p6,p7;
- computeConstantWaypoints(pData,T,n,p0,p1,p2,p3,p4,p5,p6,p7);
std::vector<bezier_t::point_t> pts;
- pts.push_back(p0);
- pts.push_back(p1);
- pts.push_back(p2);
- pts.push_back(p3);
- pts.push_back(p4);
- pts.push_back(p5);
- pts.push_back(p6);
- pts.push_back(p7);
+
+ if(n<=6){
+ point_t p0,p1,p2,p3,p4,p5;
+ computeConstantWaypoints(pData,T,n,p0,p1,p2,p3,p4,p5);
+ pts.push_back(p0);
+ pts.push_back(p1);
+ pts.push_back(p2);
+ pts.push_back(p3);
+ pts.push_back(p4);
+ pts.push_back(p5);
+ }else if(n<=8){
+ point_t p0,p1,p2,p3,p4,p5,p6,p7;
+ computeConstantWaypoints(pData,T,n,p0,p1,p2,p3,p4,p5,p6,p7);
+ pts.push_back(p0);
+ pts.push_back(p1);
+ pts.push_back(p2);
+ pts.push_back(p3);
+ pts.push_back(p4);
+ pts.push_back(p5);
+ pts.push_back(p6);
+ pts.push_back(p7);
+ }else if (n<=10){
+ point_t p0,p1,p2,p3,p4,p5,p6,p7,p8,p9;
+ computeConstantWaypoints(pData,T,n,p0,p1,p2,p3,p4,p5,p6,p7,p8,p9);
+ pts.push_back(p0);
+ pts.push_back(p1);
+ pts.push_back(p2);
+ pts.push_back(p3);
+ pts.push_back(p4);
+ pts.push_back(p5);
+ pts.push_back(p6);
+ pts.push_back(p7);
+ pts.push_back(p8);
+ pts.push_back(p9);
+ }else if (n<=12){
+ point_t p0,p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11;
+ computeConstantWaypoints(pData,T,n,p0,p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11);
+ pts.push_back(p0);
+ pts.push_back(p1);
+ pts.push_back(p2);
+ pts.push_back(p3);
+ pts.push_back(p4);
+ pts.push_back(p5);
+ pts.push_back(p6);
+ pts.push_back(p7);
+ pts.push_back(p8);
+ pts.push_back(p9);
+ pts.push_back(p10);
+ pts.push_back(p11);
+ }
return pts;
}
+std::vector<waypoint_t> createEndEffectorJerkWaypoints(double T,const ProblemData& pData){
+ // create the waypoint from the analytical expressions :
+ std::vector<waypoint_t> wps;
+ point_t p0,p1,p2,p3,p5,p6,p7,p8;
+ computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
+ double alpha = 1. / (T*T*T);
+
+ waypoint_t w = initwp<waypoint_t>();
+ // assign w0:
+ w.second= 336*(-p0 +3*p1 - 3*p2 + p3)*alpha;
+ wps.push_back(w);
+ w = initwp<waypoint_t>();
+ // assign w1:
+ w.first = 336*alpha*Matrix3::Identity();
+ w.second= 336*(-p1 + 3*p2 - 3*p3)*alpha;
+ wps.push_back(w);
+ w = initwp<waypoint_t>();
+ // assign w2:
+ w.first = -3*336*alpha*Matrix3::Identity();
+ w.second = 336*(-p2 + 3*p3 + p5)*alpha;
+ wps.push_back(w);
+ w = initwp<waypoint_t>();
+ // assign w3:
+ w.first = 3*336*alpha*Matrix3::Identity();
+ w.second = 336*(-p3 - 3*p5 + p6)*alpha;
+ wps.push_back(w);
+ w = initwp<waypoint_t>();
+ // assign w4:
+ w.first = -336*alpha*Matrix3::Identity();
+ w.second = 336*(3*p5 - 3*p6 + p7)*alpha;
+ wps.push_back(w);
+ w = initwp<waypoint_t>();
+ // assign w5:
+ w.second= 336*(-p5 + 3*p6 - 3*p7 + p8)*alpha;
+ wps.push_back(w);
+ return wps;
+}
+
std::vector<waypoint_t> createEndEffectorAccelerationWaypoints(double T,const ProblemData& pData){
// create the waypoint from the analytical expressions :
std::vector<waypoint_t> wps;
@@ -158,11 +317,14 @@ std::vector<waypoint_t> createEndEffectorVelocityWaypoints(double T,const Proble
}
-void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoint_t>& wps_acc,const std::vector<waypoint_t>& wps_vel,const VectorX& acc_bounds,const VectorX& vel_bounds,MatrixXX& A,VectorX& b){
+void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoint_t>& wps_acc,const std::vector<waypoint_t>& wps_vel,const VectorX& acc_bounds,const VectorX& vel_bounds,MatrixXX& A,VectorX& b,const std::vector<waypoint_t>& wps_jerk = std::vector<waypoint_t>(),const VectorX& jerk_bounds=VectorX(DIM_POINT) ){
assert(acc_bounds.size() == DIM_POINT && "Acceleration bounds should have the same dimension as the points");
assert(vel_bounds.size() == DIM_POINT && "Velocity bounds should have the same dimension as the points");
+ assert(jerk_bounds.size() == DIM_POINT && "Jerk bounds should have the same dimension as the points");
+
int empty_acc=0;
int empty_vel=0;
+ int empty_jerk=0;
for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit)
{
if(wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
@@ -173,8 +335,13 @@ void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoin
if(wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
empty_vel++;
}
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit)
+ {
+ if(wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
+ empty_jerk++;
+ }
- A = MatrixXX::Zero(2*DIM_POINT*(wps_acc.size()-empty_acc+wps_vel.size()-empty_vel)+DIM_POINT,DIM_POINT); // *2 because we have to put the lower and upper bound for each one, +DIM_POINT for the constraint on x[z]
+ A = MatrixXX::Zero(2*DIM_POINT*(wps_acc.size()-empty_acc+wps_vel.size()-empty_vel+wps_jerk.size() - empty_jerk)+DIM_POINT,DIM_POINT); // *2 because we have to put the lower and upper bound for each one, +DIM_POINT for the constraint on x[z]
b = VectorX::Zero(A.rows());
int i = 0;
//upper acc bounds
@@ -214,6 +381,25 @@ void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoin
}
}
+ //upper jerk bounds
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit)
+ {
+ if(! wpcit->first.isZero(std::numeric_limits<double>::epsilon())){
+ A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = wpcit->first;
+ b.segment<DIM_POINT>(i*DIM_POINT) = vel_bounds - wpcit->second;
+ ++i;
+ }
+ }
+ //lower jerk bounds
+ for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit)
+ {
+ if(! wpcit->first.isZero(std::numeric_limits<double>::epsilon())){
+ A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = -wpcit->first;
+ b.segment<DIM_POINT>(i*DIM_POINT) = jerk_bounds + wpcit->second;
+ ++i;
+ }
+ }
+
// test : constraint x[z] to be always higher than init[z] and goal[z].
// TODO replace z with the direction of the contact normal ... need to change the API
MatrixXX mxz = MatrixXX::Zero(DIM_POINT,DIM_POINT);
@@ -311,6 +497,32 @@ coefs_t evaluateCurve(const ProblemData& pData,double T, double t){
return coefs;
}
+
+/**
+ * @brief evaluateAccCurve Evaluate the acceleration at a given parameter
+ * @param pData
+ * @param T
+ * @param param Normalized : between 0 and 1
+ * @return
+ */
+coefs_t evaluateVelCurve(const ProblemData& pData, double T, double t){
+ point_t p0,p1,p2,p3,p5,p6,p7,p8;
+ computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
+ coefs_t coefs;
+ const double alpha = 1./(T);
+ const double t2 = t*t;
+ const double t3 = t2*t;
+ const double t4 = t3*t;
+ const double t5 = t4*t;
+ const double t6 = t5*t;
+ const double t7 = t6*t;
+ //equations found with sympy
+ coefs.first= (560.0*t7 - 1960.0*t6 + 2520.0*t5 - 1400.0*t4 + 280.0*t3)*alpha;
+ coefs.second=(8.0*p0*t7 - 56.0*p0*t6 + 168.0*p0*t5 - 280.0*p0*t4 + 280.0*p0*t3 - 168.0*p0*t2 + 56.0*p0*t - 8.0*p0 - 64.0*p1*t7 + 392.0*p1*t6 - 1008.0*p1*t5 + 1400.0*p1*t4 - 1120.0*p1*t3 + 504.0*p1*t2 - 112.0*p1*t + 8.0*p1 + 224.0*p2*t7 - 1176.0*p2*t6 + 2520.0*p2*t5 - 2800.0*p2*t4 + 1680.0*p2*t3 - 504.0*p2*t2 + 56.0*p2*t - 448.0*p3*t7 + 1960.0*p3*t6 - 3360.0*p3*t5 + 2800.0*p3*t4 - 1120.0*p3*t3 + 168.0*p3*t2 - 448.0*p5*t7 + 1176.0*p5*t6 - 1008.0*p5*t5 + 280.0*p5*t4 + 224.0*p6*t7 - 392.0*p6*t6 + 168.0*p6*t5 - 64.0*p7*t7 + 56.0*p7*t6 + 8.0*p8*t7)*alpha;
+ return coefs;
+}
+
+
/**
* @brief evaluateAccCurve Evaluate the acceleration at a given parameter
* @param pData
@@ -322,13 +534,37 @@ coefs_t evaluateAccCurve(const ProblemData& pData, double T, double t){
point_t p0,p1,p2,p3,p5,p6,p7,p8;
computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
coefs_t coefs;
- double alpha = 1./(T*T);
+ const double alpha = 1./(T*T);
//equations found with sympy
coefs.first= ((3920.0*pow(t,6) - 11760.0*pow(t,5) + 12600.0*pow(t,4) - 5600.0*pow(t,3) + 840.0*pow(t,2)))*alpha;
coefs.second=(56.0*p0*pow(t,6) - 336.0*p0*pow(t,5) + 840.0*p0*pow(t,4) - 1120.0*p0*pow(t,3) + 840.0*p0*pow(t,2) - 336.0*p0*t + 56.0*p0 - 448.0*p1*pow(t,6) + 2352.0*p1*pow(t,5) - 5040.0*p1*pow(t,4) + 5600.0*p1*pow(t,3) - 3360.0*p1*pow(t,2) + 1008.0*p1*t - 112.0*p1 + 1568.0*p2*pow(t,6) - 7056.0*p2*pow(t,5) + 12600.0*p2*pow(t,4) - 11200.0*p2*pow(t,3) + 5040.0*p2*pow(t,2) - 1008.0*p2*t + 56.0*p2 - 3136.0*p3*pow(t,6) + 11760.0*p3*pow(t,5) - 16800.0*p3*pow(t,4) + 11200.0*p3*pow(t,3) - 3360.0*p3*pow(t,2)+ 336.0*p3*t - 3136.0*p5*pow(t,6) + 7056.0*p5*pow(t,5) - 5040.0*p5*pow(t,4) + 1120.0*p5*pow(t,3) + 1568.0*p6*pow(t,6) - 2352.0*p6*pow(t,5) + 840.0*p6*pow(t,4) - 448.0*p7*pow(t,6) + 336.0*p7*pow(t,5) + 56.0*p8*pow(t,6))*alpha;
return coefs;
}
+
+/**
+ * @brief evaluateAccCurve Evaluate the acceleration at a given parameter
+ * @param pData
+ * @param T
+ * @param param Normalized : between 0 and 1
+ * @return
+ */
+coefs_t evaluateJerkCurve(const ProblemData& pData, double T, double t){
+ point_t p0,p1,p2,p3,p5,p6,p7,p8;
+ computeConstantWaypoints(pData,T,8,p0,p1,p2,p3,p5,p6,p7,p8);
+ coefs_t coefs;
+ const double t2 = t*t;
+ const double t3 = t2*t;
+ const double t4 = t3*t;
+ const double t5 = t4*t;
+ const double alpha = 1./(T*T*T);
+
+ //equations found with sympy
+ coefs.first= (23520.0*t5 - 58800.0*t4 + 50400.0*t3 - 16800.0*t2 + 1680.0*t)*alpha;
+ coefs.second= 1.0*(336.0*p0*t5 - 1680.0*p0*t4 + 3360.0*p0*t3 - 3360.0*p0*t2 + 1680.0*p0*t - 336.0*p0 - 2688.0*p1*t5 + 11760.0*p1*t4 - 20160.0*p1*t3 + 16800.0*p1*t2 - 6720.0*p1*t + 1008.0*p1 + 9408.0*p2*t5 - 35280.0*p2*t4 + 50400.0*p2*t3 - 33600.0*p2*t2 + 10080.0*p2*t - 1008.0*p2 - 18816.0*p3*t5 + 58800.0*p3*t4 - 67200.0*p3*t3 + 33600.0*p3*t2 - 6720.0*p3*t + 336.0*p3 - 18816.0*p5*t5 + 35280.0*p5*t4 - 20160.0*p5*t3 + 3360.0*p5*t2 + 9408.0*p6*t5 - 11760.0*p6*t4 + 3360.0*p6*t3 - 2688.0*p7*t5 + 1680.0*p7*t4 + 336.0*p8*t5)*alpha;
+ return coefs;
+}
+
template <typename Path>
void computeDistanceCostFunction(int numPoints,const ProblemData& pData, double T,const Path& path, MatrixXX& H,VectorX& g){
H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
@@ -349,6 +585,9 @@ void computeDistanceCostFunction(int numPoints,const ProblemData& pData, double
g += (ckcit->first * ckcit->second) - (pk * ckcit->first);
i++;
}
+ double norm=H(0,0); // because H is always diagonal.
+ H /= norm;
+ g /= norm;
}
void computeC_of_T (const ProblemData& pData,double T, ResultDataCOMTraj& res){
@@ -368,6 +607,27 @@ void computeC_of_T (const ProblemData& pData,double T, ResultDataCOMTraj& res){
std::cout<<"bezier curve created, size = "<<res.c_of_t_.size_<<std::endl;
}
+void computeVelCostFunction(int numPoints,const ProblemData& pData,double T, MatrixXX& H,VectorX& g){
+ double step = 1./(numPoints-1);
+ H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
+ g = VectorX::Zero(DIM_POINT);
+ std::vector<coefs_t> cks;
+ for(int i = 0 ; i < numPoints ; ++i){
+ cks.push_back(evaluateVelCurve(pData,T,i*step));
+ }
+ for (std::vector<coefs_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit){
+ H+=(ckcit->first * ckcit->first * Matrix3::Identity());
+ g+=ckcit->first*ckcit->second;
+ }
+ //TEST : don't consider z axis for minimum acceleration cost
+ //H(2,2) = 1e-6;
+ //g[2] = 1e-6 ;
+ //normalize :
+ double norm=H(0,0); // because H is always diagonal
+ H /= norm;
+ g /= norm;
+}
+
void computeAccelerationCostFunction(int numPoints,const ProblemData& pData,double T, MatrixXX& H,VectorX& g){
double step = 1./(numPoints-1);
H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
@@ -381,12 +641,33 @@ void computeAccelerationCostFunction(int numPoints,const ProblemData& pData,doub
g+=ckcit->first*ckcit->second;
}
//TEST : don't consider z axis for minimum acceleration cost
- H(2,2) = 1e-6;
- g[2] = 1e-6 ;
+ //H(2,2) = 1e-6;
+ //g[2] = 1e-6 ;
//normalize :
- // double norm=H(0,0); // because H is always diagonal
- // H /= norm;
- // g /= norm;
+ double norm=H(0,0); // because H is always diagonal
+ H /= norm;
+ g /= norm;
+}
+
+void computeJerkCostFunction(int numPoints,const ProblemData& pData,double T, MatrixXX& H,VectorX& g){
+ double step = 1./(numPoints-1);
+ H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
+ g = VectorX::Zero(DIM_POINT);
+ std::vector<coefs_t> cks;
+ for(int i = 0 ; i < numPoints ; ++i){
+ cks.push_back(evaluateJerkCurve(pData,T,i*step));
+ }
+ for (std::vector<coefs_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit){
+ H+=(ckcit->first * ckcit->first * Matrix3::Identity());
+ g+=ckcit->first*ckcit->second;
+ }
+ //TEST : don't consider z axis for minimum acceleration cost
+ //H(2,2) = 1e-6;
+ //g[2] = 1e-6 ;
+ //normalize :
+ double norm=H(0,0); // because H is always diagonal
+ H /= norm;
+ g /= norm;
}
@@ -395,32 +676,36 @@ ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, co
std::cout<<"solve end effector, T = "<<T<<std::endl;
assert (weightDistance>=0. && weightDistance<=1. && "WeightDistance must be between 0 and 1");
double weightAcc = 1. - weightDistance;
+ std::vector<waypoint_t> wps_jerk=createEndEffectorJerkWaypoints(T,pData);
std::vector<waypoint_t> wps_acc=createEndEffectorAccelerationWaypoints(T,pData);
std::vector<waypoint_t> wps_vel=createEndEffectorVelocityWaypoints(T,pData);
// stack the constraint for each waypoint :
MatrixXX A;
VectorX b;
- Vector3 acc_bounds(1000,1000,1000);
- Vector3 vel_bounds(500,500,500);
- computeConstraintsMatrix(pData,wps_acc,wps_vel,acc_bounds,vel_bounds,A,b);
+ Vector3 jerk_bounds(500,500,500);
+ Vector3 acc_bounds(30,30,30);
+ Vector3 vel_bounds(20,20,20);
+ computeConstraintsMatrix(pData,wps_acc,wps_vel,acc_bounds,vel_bounds,A,b,wps_jerk,jerk_bounds);
// std::cout<<"End eff A = "<<std::endl<<A<<std::endl;
// std::cout<<"End eff b = "<<std::endl<<b<<std::endl;
// compute cost function (discrete integral under the curve defined by 'path')
- MatrixXX H_rrt=MatrixXX::Zero(DIM_POINT,DIM_POINT),H_acc,H;
- VectorX g_rrt=VectorX::Zero(DIM_POINT),g_acc,g;
+ MatrixXX H_rrt=MatrixXX::Zero(DIM_POINT,DIM_POINT),H_acc,H_jerk,H_vel,H;
+ VectorX g_rrt=VectorX::Zero(DIM_POINT),g_acc,g_jerk,g_vel,g;
if(weightDistance>0)
- computeDistanceCostFunction<Path>(20,pData,T,path,H_rrt,g_rrt);
- computeAccelerationCostFunction(50,pData,T,H_acc,g_acc);
+ computeDistanceCostFunction<Path>(50,pData,T,path,H_rrt,g_rrt);
+ computeVelCostFunction(50,pData,T,H_vel,g_vel);
+ // computeJerkCostFunction(50,pData,T,H_jerk,g_jerk);
/* std::cout<<"End eff H_rrt = "<<std::endl<<H_rrt<<std::endl;
std::cout<<"End eff g_rrt = "<<std::endl<<g_rrt<<std::endl;
std::cout<<"End eff H_acc = "<<std::endl<<H_acc<<std::endl;
std::cout<<"End eff g_acc = "<<std::endl<<g_acc<<std::endl;
*/
+
// add the costs :
H = MatrixXX::Zero(DIM_POINT,DIM_POINT);
g = VectorX::Zero(DIM_POINT);
- H = weightAcc*H_acc + weightDistance*H_rrt;
- g = weightAcc*g_acc + weightDistance*g_rrt;
+ H = weightAcc*(/*H_jerk+*/H_vel) + weightDistance*H_rrt;
+ g = weightAcc*(/*g_jerk+*/g_vel) + weightDistance*g_rrt;
std::cout<<"End eff H = "<<std::endl<<H<<std::endl;
std::cout<<"End eff g = "<<std::endl<<g<<std::endl;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
new file mode 100644
index 0000000000000000000000000000000000000000..6a95c0cd2dc4ce55d9b4259b9c0879e4f954350c
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
@@ -0,0 +1,151 @@
+#ifndef BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
+#define BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
+
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1{
+
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK;
+
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION (DEGREE = 6)
+///
+/**
+ * @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one free waypoint (x)
+ * @param pi constant waypoints of the curve, assume p0 p1 p2 x p3 p4 p5
+ * @param t param (normalized !)
+ * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ */
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
+ coefs_t wp;
+ double t2 = t*t;
+ double t3 = t2*t;
+ double t4 = t3*t;
+ double t5 = t4*t;
+ double t6 = t5*t;
+ // equation found with sympy
+ wp.first = -20.0*t6 + 60.0*t5 - 60.0*t4 + 20.0*t3;
+ wp.second = 1.0*pi[0]*t6 - 6.0*pi[0]*t5 + 15.0*pi[0]*t4 - 20.0*pi[0]*t3 + 15.0*pi[0]*t2 - 6.0*pi[0]*t + 1.0*pi[0] - 6.0*pi[1]*t6 + 30.0*pi[1]*t5 - 60.0*pi[1]*t4 + 60.0*pi[1]*t3 - 30.0*pi[1]*t2 + 6.0*pi[1]*t + 15.0*pi[2]*t6 - 60.0*pi[2]*t5 + 90.0*pi[2]*t4 - 60.0*pi[2]*t3 + 15.0*pi[2]*t2 + 15.0*pi[4]*t6 - 30.0*pi[4]*t5 + 15.0*pi[4]*t4 - 6.0*pi[5]*t6 + 6.0*pi[5]*t5 + 1.0*pi[6]*t6;
+ return wp;
+}
+
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
+ coefs_t wp;
+ double alpha = 1./(T*T);
+ double t2 = t*t;
+ double t3 = t2*t;
+ double t4 = t3*t;
+ // equation found with sympy
+ wp.first = 1.0*(-600.0*t4 + 1200.0*t3 - 720.0*t2 + 120.0*t)*alpha;
+ wp.second = 1.0*(30.0*pi[0]*t4 - 120.0*pi[0]*t3 + 180.0*pi[0]*t2 - 120.0*pi[0]*t + 30.0*pi[0] - 180.0*pi[1]*t4 + 600.0*pi[1]*t3 - 720.0*pi[1]*t2 + 360.0*pi[1]*t - 60.0*pi[1] + 450.0*pi[2]*t4 - 1200.0*pi[2]*t3 + 1080.0*pi[2]*t2 - 360.0*pi[2]*t + 30.0*pi[2] + 450.0*pi[4]*t4 - 600.0*pi[4]*t3 + 180.0*pi[4]*t2 - 180.0*pi[5]*t4 + 120.0*pi[5]*t3 + 30.0*pi[6]*t4)*alpha;
+ return wp;
+}
+
+
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+ // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant waypoint and one free (p3))
+ // first, compute the constant waypoints that only depend on pData :
+ double n = 6.;
+ std::vector<point_t> pi;
+ pi.push_back(pData.c0_); //p0
+ pi.push_back((pData.dc0_ * T / n )+ pData.c0_); // p1
+ pi.push_back((pData.ddc0_*T*T/(n*(n-1))) + (2.*pData.dc0_ *T / n) + pData.c0_); // p2
+ pi.push_back(point_t::Zero()); // x
+ pi.push_back((pData.ddc1_*T*T/(n*(n-1))) - (2*pData.dc1_*T/n) + pData.c1_) ;
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
+ pi.push_back(pData.c1_); // p5
+ return pi;
+}
+
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+ std::vector<waypoint6_t> wps;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ std::vector<Matrix3> Cpi;
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
+ Cpi.push_back(skew(pi[i]));
+ }
+ const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
+ const Matrix3 Cg = skew( g);
+ const double T2 = T*T;
+ const double alpha = 1/(T2);
+
+ // equation of waypoints for curve w found with sympy
+ waypoint6_t w0 = initwp<waypoint6_t>();
+ w0.second.head<3>() = (30*pi[0] - 60*pi[1] + 30*pi[2])*alpha;
+ w0.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[0] - 60.0*Cpi[0]*pi[1] + 30.0*Cpi[0]*pi[2])*alpha;
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = 13.3333333333333*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) = 13.3333333333333*Cpi[0]*alpha;
+ w1.second.head<3>() = 1.0*(16.6666666666667*pi[0] - 20.0*pi[1] - 10.0*pi[2])*alpha;
+ w1.second.tail<3>() = 1.0*(0.333333333333333*Cg*T2*pi[0] + 0.666666666666667*Cg*T2*pi[1] - 30.0*Cpi[0]*pi[2] + 20.0*Cpi[1]*pi[2])*alpha;
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = 6.66666666666667*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = 1.0*(-13.3333333333333*Cpi[0] + 20.0*Cpi[1])*alpha;
+ w2.second.head<3>() = 1.0*(8.33333333333333*pi[0] - 20.0*pi[2] + 5.0*pi[4])*alpha;
+ w2.second.tail<3>() = 1.0*(0.0833333333333334*Cg*T2*pi[0] + 0.5*Cg*T2*pi[1] + 0.416666666666667*Cg*T2*pi[2] + 5.0*Cpi[0]*pi[4] - 20.0*Cpi[1]*pi[2])*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -5.71428571428572*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = 1.0*(0.238095238095238*Cg*T2 - 20.0*Cpi[1] + 14.2857142857143*Cpi[2])*alpha;
+ w3.second.head<3>() = 1.0*(3.57142857142857*pi[0] + 7.14285714285714*pi[1] - 14.2857142857143*pi[2] + 7.85714285714286*pi[4] + 1.42857142857143*pi[5])*alpha;
+ w3.second.tail<3>() = 1.0*(0.0119047619047619*Cg*T2*pi[0] + 0.214285714285714*Cg*T2*pi[1] + 0.535714285714286*Cg*T2*pi[2] - 5.0*Cpi[0]*pi[4] + 1.42857142857143*Cpi[0]*pi[5] + 12.8571428571429*Cpi[1]*pi[4])*alpha;
+ wps.push_back(w3);
+ waypoint6_t w4 = initwp<waypoint6_t>();
+ w4.first.block<3,3>(0,0) = -14.2857142857143*alpha*Matrix3::Identity();
+ w4.first.block<3,3>(3,0) = 1.0*(0.476190476190476*Cg*T2 - 14.2857142857143*Cpi[2])*alpha;
+ w4.second.head<3>() = 1.0*(1.19047619047619*pi[0] + 7.14285714285714*pi[1] - 3.57142857142857*pi[2] + 5.0*pi[4] + 4.28571428571429*pi[5] + 0.238095238095238*pi[6])*alpha;
+ w4.second.tail<3>() = 1.0*( 0.0476190476190471*Cg*T2*pi[1] + 0.357142857142857*Cg*T2*pi[2] + 0.119047619047619*Cg*T2*pi[4] - 1.42857142857143*Cpi[0]*pi[5] + 0.238095238095238*Cpi[0]*pi[6] - 12.8571428571429*Cpi[1]*pi[4] + 5.71428571428571*Cpi[1]*pi[5] + 17.8571428571429*Cpi[2]*pi[4])*alpha;
+ wps.push_back(w4);
+ waypoint6_t w5 = initwp<waypoint6_t>();
+ w5.first.block<3,3>(0,0) = -14.2857142857143*alpha*Matrix3::Identity();
+ w5.first.block<3,3>(3,0) = 1.0*(0.476190476190476*Cg*T2 - 14.2857142857143*Cpi[4])*alpha;
+ w5.second.head<3>() = 1.0*(0.238095238095238*pi[0] + 4.28571428571429*pi[1] + 5.0*pi[2] - 3.57142857142857*pi[4] + 7.14285714285714*pi[5] + 1.19047619047619*pi[6])*alpha;
+ w5.second.tail<3>() = 1.0*( + 0.11904761904762*Cg*T2*pi[2] + 0.357142857142857*Cg*T2*pi[4] + 0.0476190476190476*Cg*T2*pi[5] - 0.238095238095238*Cpi[0]*pi[6] - 5.71428571428572*Cpi[1]*pi[5] + 1.42857142857143*Cpi[1]*pi[6] - 17.8571428571429*Cpi[2]*pi[4] + 12.8571428571429*Cpi[2]*pi[5])*alpha;
+ wps.push_back(w5);
+ waypoint6_t w6 = initwp<waypoint6_t>();
+ w6.first.block<3,3>(0,0) = -5.71428571428571*alpha*Matrix3::Identity();
+ w6.first.block<3,3>(3,0) = 1.0*(0.238095238095238*Cg*T2 + 14.2857142857143*Cpi[4] - 20.0*Cpi[5])*alpha;
+ w6.second.head<3>() = 1.0*(1.42857142857143*pi[1] + 7.85714285714286*pi[2] - 14.2857142857143*pi[4] + 7.14285714285715*pi[5] + 3.57142857142857*pi[6])*alpha;
+ w6.second.tail<3>() = 1.0*(0.535714285714286*Cg*T2*pi[4] + 0.214285714285714*Cg*T2*pi[5] + 0.0119047619047619*Cg*T2*pi[6] - 1.42857142857143*Cpi[1]*pi[6] - 12.8571428571429*Cpi[2]*pi[5] + 5.0*Cpi[2]*pi[6])*alpha;
+ wps.push_back(w6);
+ waypoint6_t w7 = initwp<waypoint6_t>();
+ w7.first.block<3,3>(0,0) = 6.66666666666667*alpha*Matrix3::Identity();
+ w7.first.block<3,3>(3,0) = 1.0*( 20.0*Cpi[5] - 13.3333333333333*Cpi[6])*alpha;
+ w7.second.head<3>() = 1.0*(5.0*pi[2] - 20.0*pi[4] + 8.33333333333333*pi[6])*alpha;
+ w7.second.tail<3>() = 1.0*( 0.416666666666667*Cg*T2*pi[4] + 0.5*Cg*T2*pi[5] + 0.0833333333333333*Cg*T2*pi[6] - 5.0*Cpi[2]*pi[6] + 20.0*Cpi[4]*pi[5])*alpha;
+ wps.push_back(w7);
+ waypoint6_t w8 = initwp<waypoint6_t>();
+ w8.first.block<3,3>(0,0) = 13.3333333333333*alpha*Matrix3::Identity();
+ w8.first.block<3,3>(3,0) = 1.0*( 13.3333333333333*Cpi[6])*alpha;
+ w8.second.head<3>() = 1.0*(-9.99999999999999*pi[4] - 20.0*pi[5] + 16.6666666666667*pi[6])*alpha;
+ w8.second.tail<3>() = 1.0*( 0.666666666666667*Cg*T2*pi[5] + 0.333333333333333*Cg*T2*pi[6] - 20.0*Cpi[4]*pi[5] + 30.0*Cpi[4]*pi[6])*alpha;
+ wps.push_back(w8);
+ waypoint6_t w9 = initwp<waypoint6_t>();
+ w9.second.head<3>() = (30*pi[4] - 60*pi[5] + 30*pi[6])*alpha;
+ w9.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[6] - 30.0*Cpi[4]*pi[6] + 60.0*Cpi[5]*pi[6])*alpha;
+ wps.push_back(w9);
+ return wps;
+}
+
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-6.0*pi[5] + 6.0*pi[6])/ T;
+ return v;
+}
+
+}
+
+}
+
+
+#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
index dfa681b495c8449d91e7db354a13545d31aa4046..0dc6612f0ee23cd6dbe6a7166e0029bbb5cc130e 100644
--- a/src/waypoints_definition.cpp
+++ b/src/waypoints_definition.cpp
@@ -14,6 +14,8 @@
#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 <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh>
+
#include "boost/assign.hpp"