diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index 3ae8b7f276385eeea0732e8e9e181144cb9af2c2..5a0f5a11720ae6f70df3aad22696f96b040f0ad5 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -150,114 +150,230 @@ std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
wps.push_back(w5);
return wps;
}
+#endif // deg 4 : constraints on c0 dc0 x dc1 c1
+#if (DDC0_CONSTRAINT && DC1_CONSTRAINT)
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND INIT ACCELERATION (DEGREE = 5)
///
+/**
+ * @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 x p2 p3
+ * @param t param (normalized !)
+ * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ */
+coefs_t evaluateCurveAtTime(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;
+ // equation found with sympy
+ wp.first = 10.0*t5 - 20.0*t4 + 10.0*t3;
+ wp.second = -1.0*pi[0]*t5 + 5.0*pi[0]*t4 - 10.0*pi[0]*t3 + 10.0*pi[0]*t2 - 5.0*pi[0]*t + 1.0*pi[0] + 5.0*pi[1]*t5 - 20.0*pi[1]*t4 + 30.0*pi[1]*t3 - 20.0*pi[1]*t2 + 5.0*pi[1]*t - 10.0*pi[2]*t5 + 30.0*pi[2]*t4 - 30.0*pi[2]*t3 + 10.0*pi[2]*t2 - 5.0*pi[4]*t5 + 5.0*pi[4]*t4 + 1.0*pi[5]*t5;
+ // std::cout<<"wp at t = "<<t<<std::endl;
+ // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
+ return wp;
+}
-///**
-// * @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 x p2 p3
-// * @param t param (normalized !)
-// * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
-// */
-//coefs_t evaluateCurveAtTime(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;
-// // equation found with sympy
-// wp.first = 10.0*t5 - 20.0*t4 + 10.0*t3;
-// wp.second = -1.0*pi[0]*t5 + 5.0*pi[0]*t4 - 10.0*pi[0]*t3 + 10.0*pi[0]*t2 - 5.0*pi[0]*t + 1.0*pi[0] + 5.0*pi[1]*t5 - 20.0*pi[1]*t4 + 30.0*pi[1]*t3 - 20.0*pi[1]*t2 + 5.0*pi[1]*t - 10.0*pi[2]*t5 + 30.0*pi[2]*t4 - 30.0*pi[2]*t3 + 10.0*pi[2]*t2 - 5.0*pi[4]*t5 + 5.0*pi[4]*t4 + 1.0*pi[5]*t5;
-// // std::cout<<"wp at t = "<<t<<std::endl;
-// // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
-// return wp;
-//}
+coefs_t evaluateAccelerationCurveAtTime(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;
+ // equation found with sympy
+ wp.first = (200.0*t3 - 240.0*t2 + 60.0*t)*alpha;
+ wp.second = 1.0*(-20.0*pi[0]*t3 + 60.0*pi[0]*t2 - 60.0*pi[0]*t + 20.0*pi[0] + 100.0*pi[1]*t3 - 240.0*pi[1]*t2 + 180.0*pi[1]*t - 40.0*pi[1] - 200.0*pi[2]*t3 + 360.0*pi[2]*t2 - 180.0*pi[2]*t + 20.0*pi[2] - 100.0*pi[4]*t3 + 60.0*pi[4]*t2 + 20.0*pi[5]*t3)*alpha;
+ // std::cout<<"acc_wp at t = "<<t<<std::endl;
+ // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
+ return wp;
+}
-//coefs_t evaluateAccelerationCurveAtTime(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;
-// // equation found with sympy
-// wp.first = (200.0*t3 - 240.0*t2 + 60.0*t)*alpha;
-// wp.second = 1.0*(-20.0*pi[0]*t3 + 60.0*pi[0]*t2 - 60.0*pi[0]*t + 20.0*pi[0] + 100.0*pi[1]*t3 - 240.0*pi[1]*t2 + 180.0*pi[1]*t - 40.0*pi[1] - 200.0*pi[2]*t3 + 360.0*pi[2]*t2 - 180.0*pi[2]*t + 20.0*pi[2] - 100.0*pi[4]*t3 + 60.0*pi[4]*t2 + 20.0*pi[5]*t3)*alpha;
-// // std::cout<<"acc_wp at t = "<<t<<std::endl;
-// // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
-// return wp;
-//}
+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 = 5.;
+ 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.dc1_ * T / n) + pData.c1_); // p4
+ pi.push_back(pData.c1_); // p5
+ /*std::cout<<"fixed waypoints : "<<std::endl;
+ for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
+ std::cout<<" pi = "<<*pit<<std::endl;
+ }*/
+ return pi;
+}
-//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 = 5.;
-// 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.dc1_ * T / n) + pData.c1_); // p4
-// pi.push_back(pData.c1_); // p5
-// /*std::cout<<"fixed waypoints : "<<std::endl;
-// for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
-// std::cout<<" pi = "<<*pit<<std::endl;
-// }*/
-// return pi;
-//}
+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(int 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>() = (20*pi[0] - 40*pi[1] + 20*pi[2])*alpha;
+ w0.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[0] - 40.0*Cpi[0]*pi[1] + 20.0*Cpi[0]*pi[2])*alpha;
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = 8.57142857142857*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) = 8.57142857142857*Cpi[0]*alpha;
+ w1.second.head<3>() = 1.0*(11.4285714285714*pi[0] - 14.2857142857143*pi[1] - 5.71428571428572*pi[2])*alpha;
+ w1.second.tail<3>() = 1.0*(0.285714285714286*Cg*T2*pi[0] + 0.714285714285714*Cg*T2*pi[1] - 20.0*Cpi[0]*pi[2] + 14.2857142857143*Cpi[1]*pi[2])*alpha;
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = 5.71428571428571*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = 1.0*(-8.57142857142857*Cpi[0] + 14.2857142857143*Cpi[1])*alpha;
+ w2.second.head<3>() = 1.0*(5.71428571428571*pi[0] - 14.2857142857143*pi[2] + 2.85714285714286*pi[4])*alpha;
+ w2.second.tail<3>() = 1.0*(0.0476190476190479*Cg*T2*pi[0] + 0.476190476190476*Cg*T2*pi[1] + 0.476190476190476*Cg*T2*pi[2] + 2.85714285714286*Cpi[0]*pi[4] - 14.2857142857143*Cpi[1]*pi[2])*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -2.85714285714286*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = 1.0*(0.285714285714286*Cg*T2 - 14.2857142857143*Cpi[1] + 11.4285714285714*Cpi[2])*alpha;
+ w3.second.head<3>() = 1.0*(2.28571428571429*pi[0] + 5.71428571428571*pi[1] - 11.4285714285714*pi[2] + 5.71428571428571*pi[4] + 0.571428571428571*pi[5])*alpha;
+ w3.second.tail<3>() = 1.0*(0.142857142857143*Cg*T2*pi[1] + 0.571428571428571*Cg*T2*pi[2] - 2.85714285714286*Cpi[0]*pi[4] + 0.571428571428571*Cpi[0]*pi[5] + 8.57142857142857*Cpi[1]*pi[4])*alpha;
+ wps.push_back(w3);
+ waypoint6_t w4 = initwp<waypoint6_t>();
+ w4.first.block<3,3>(0,0) = -11.4285714285714*alpha*Matrix3::Identity();
+ w4.first.block<3,3>(3,0) = 1.0*(0.571428571428571*Cg*T2 - 11.4285714285714*Cpi[2])*alpha;
+ w4.second.head<3>() = 1.0*(0.571428571428571*pi[0] + 5.71428571428571*pi[1] - 2.85714285714286*pi[2] + 5.71428571428571*pi[4] + 2.28571428571429*pi[5])*alpha;
+ w4.second.tail<3>() = 1.0*(0.285714285714286*Cg*T2*pi[2] + 0.142857142857143*Cg*T2*pi[4] - 0.571428571428572*Cpi[0]*pi[5] - 8.57142857142857*Cpi[1]*pi[4] + 2.85714285714286*Cpi[1]*pi[5] + 14.2857142857143*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*(2.85714285714286*pi[1] + 5.71428571428571*pi[2] + 5.71428571428571*pi[5])*alpha;
+ w5.second.tail<3>() = 1.0*(0.476190476190476*Cg*T2*pi[4] + 0.0476190476190476*Cg*T2*pi[5] - 2.85714285714286*Cpi[1]*pi[5] - 14.2857142857143*Cpi[2]*pi[4] + 8.57142857142857*Cpi[2]*pi[5])*alpha;
+ wps.push_back(w5);
+ waypoint6_t w6 = initwp<waypoint6_t>();
+ w6.first.block<3,3>(0,0) = -5.71428571428572*alpha*Matrix3::Identity();
+ w6.first.block<3,3>(3,0) = 1.0*(14.2857142857143*Cpi[4] - 20.0*Cpi[5])*alpha;
+ w6.second.head<3>() = 1.0*(8.57142857142857*pi[2] - 14.2857142857143*pi[4] + 11.4285714285714*pi[5])*alpha;
+ w6.second.tail<3>() = 1.0*(0.714285714285714*Cg*T2*pi[4] + 0.285714285714286*Cg*T2*pi[5] - 8.57142857142858*Cpi[2]*pi[5])*alpha;
+ wps.push_back(w6);
+ waypoint6_t w7 = initwp<waypoint6_t>();
+ w7.first.block<3,3>(0,0) = 20*alpha*Matrix3::Identity();
+ w7.first.block<3,3>(3,0) = 1.0*(20.0*Cpi[5])*alpha;
+ w7.second.head<3>() = (-40*pi[4] + 20*pi[5])*alpha;
+ w7.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[5] + 40.0*Cpi[4]*pi[5])*alpha;
+ wps.push_back(w7);
+ return wps;
+}
+
+#endif // deg 5 : constraints on c0 dc0 ddc0 x dc1 c1
+
+#if (DDC0_CONSTRAINT && !DC1_CONSTRAINT)
/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration, and only final position (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)
-// * @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
-// * @param t param (normalized !)
-// * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
-// */
-//coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
-// coefs_t wp;
-// double t2 = t*t;
-// double t3 = t2*t;
-// double t4 = t3*t;
-// // equation found with sympy
-// wp.first = -4.0*t4 + 4.0*t3;
-// wp.second =1.0*pi[0]*t4 - 4.0*pi[0]*t3 + 6.0*pi[0]*t2 - 4.0*pi[0]*t + 1.0*pi[0] - 4.0*pi[1]*t4 + 12.0*pi[1]*t3 - 12.0*pi[1]*t2 + 4.0*pi[1]*t + 6.0*pi[2]*t4 - 12.0*pi[2]*t3 + 6.0*pi[2]*t2 + 1.0*pi[4]*t4;
-// std::cout<<"wp at t = "<<t<<std::endl;
-// std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
-// return wp;
-//}
+/**
+ * @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 x p2 p3
+ * @param t param (normalized !)
+ * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ */
+coefs_t evaluateCurveAtTime(std::vector<point_t> pi,double t){
+ coefs_t wp;
+ double t2 = t*t;
+ double t3 = t2*t;
+ double t4 = t3*t;
+ // equation found with sympy
+ wp.first = -4.0*t4 + 4.0*t3;
+ wp.second =1.0*pi[0]*t4 - 4.0*pi[0]*t3 + 6.0*pi[0]*t2 - 4.0*pi[0]*t + 1.0*pi[0] - 4.0*pi[1]*t4 + 12.0*pi[1]*t3 - 12.0*pi[1]*t2 + 4.0*pi[1]*t + 6.0*pi[2]*t4 - 12.0*pi[2]*t3 + 6.0*pi[2]*t2 + 1.0*pi[4]*t4;
+ std::cout<<"wp at t = "<<t<<std::endl;
+ std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
+ return wp;
+}
-//coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
-// coefs_t wp;
-// double alpha = 1./(T*T);
-// double t2 = t*t;
-// // equation found with sympy
-// wp.first = (-48.0*t2 + 24.0*t)*alpha;
-// wp.second = (12.0*pi[0]*t2 - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t2 + 72.0*pi[1]*t - 24.0*pi[1] + 72.0*pi[2]*t2 - 72.0*pi[2]*t + 12.0*pi[2] + 12.0*pi[4]*t2)*alpha;
-// std::cout<<"acc_wp at t = "<<t<<std::endl;
-// std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
-// return wp;
-//}
+coefs_t evaluateAccelerationCurveAtTime(std::vector<point_t> pi,double T,double t){
+ coefs_t wp;
+ double alpha = 1./(T*T);
+ double t2 = t*t;
+ // equation found with sympy
+ wp.first = (-48.0*t2 + 24.0*t)*alpha;
+ wp.second = (12.0*pi[0]*t2 - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t2 + 72.0*pi[1]*t - 24.0*pi[1] + 72.0*pi[2]*t2 - 72.0*pi[2]*t + 12.0*pi[2] + 12.0*pi[4]*t2)*alpha;
+ std::cout<<"acc_wp at t = "<<t<<std::endl;
+ std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
+ return wp;
+}
-//std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
-// // equation for constraint on initial position, velocity and acceleration, and only final position (degree = 4)(degree 4, 4 constant waypoint and one free (p3))
-// // first, compute the constant waypoints that only depend on pData :
-// double n = 4.;
-// 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.c1_); // p4
-// std::cout<<"fixed waypoints : "<<std::endl;
-// for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
-// std::cout<<" pi = "<<*pit<<std::endl;
-// }
-// return pi;
-//}
+std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+ // equation for constraint on initial position, velocity and acceleration, and only final position (degree = 4)(degree 4, 4 constant waypoint and one free (p3))
+ // first, compute the constant waypoints that only depend on pData :
+ double n = 4.;
+ 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.c1_); // p4
+ std::cout<<"fixed waypoints : "<<std::endl;
+ for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
+ std::cout<<" pi = "<<*pit<<std::endl;
+ }
+ return pi;
+}
+
+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(int 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>() = (12*pi[0] - 24*pi[1] + 12*pi[2])*alpha;
+ w0.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[0] - 24.0*Cpi[0]*pi[1] + 12.0*Cpi[0]*pi[2])*alpha;
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = 4.8*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) = 4.8*Cpi[0]*alpha;
+ w1.second.head<3>() = 1.0*(7.2*pi[0] - 9.6*pi[1] - 2.4*pi[2])*alpha;
+ w1.second.tail<3>() = 1.0*(0.2*Cg*T2*pi[0] + 0.8*Cg*T2*pi[1] - 12.0*Cpi[0]*pi[2] + 9.6*Cpi[1]*pi[2])*alpha;
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = 4.8*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = 1.0*(-4.8*Cpi[0] + 9.6*Cpi[1])*alpha;
+ w2.second.head<3>() = 1.0*(3.6*pi[0] - 9.6*pi[2] + 1.2*pi[4])*alpha;
+ w2.second.tail<3>() = 1.0*(0.4*Cg*T2*pi[1] + 0.6*Cg*T2*pi[2] + 1.2*Cpi[0]*pi[4] - 9.6*Cpi[1]*pi[2])*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(3,0) = 1.0*(0.4*Cg*T2 - 9.6*Cpi[1] + 9.6*Cpi[2])*alpha;
+ w3.second.head<3>() = 1.0*(1.2*pi[0] + 4.8*pi[1] - 9.6*pi[2] + 3.6*pi[4])*alpha;
+ w3.second.tail<3>() = 1.0*(0.6*Cg*T2*pi[2] - 1.2*Cpi[0]*pi[4] + 4.8*Cpi[1]*pi[4])*alpha;
+ wps.push_back(w3);
+ waypoint6_t w4 = initwp<waypoint6_t>();
+ w4.first.block<3,3>(0,0) = -9.6*alpha*Matrix3::Identity();
+ w4.first.block<3,3>(3,0) = 1.0*(0.8*Cg*T2 - 9.6*Cpi[2])*alpha;
+ w4.second.head<3>() = 1.0*(4.8*pi[1] - 2.4*pi[2] + 7.2*pi[4])*alpha;
+ w4.second.tail<3>() = 1.0*(0.2*Cg*T2*pi[4] - 4.8*Cpi[1]*pi[4] + 12.0*Cpi[2]*pi[4])*alpha;
+ wps.push_back(w4);
+ waypoint6_t w5 = initwp<waypoint6_t>();
+ w5.first.block<3,3>(0,0) = -24*alpha*Matrix3::Identity();
+ w5.first.block<3,3>(3,0) = 1.0*(- 24.0*Cpi[4])*alpha;
+ w5.second.head<3>() = (12*pi[2] + 12*pi[4])*alpha;
+ w5.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[4] - 12.0*Cpi[2]*pi[4])*alpha;
+ wps.push_back(w5);
+ return wps;
+}
+#endif // constraints on c0 dc0 ddc0 x c1
//coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){