diff --git a/include/bezier-com-traj/common_solve_methods.hh b/include/bezier-com-traj/common_solve_methods.hh
index 6a71938131f310cfaa3128faeabdcac743ca3ec5..bd27500b5c56e0f2d37e79b1dc28b76776867caf 100644
--- a/include/bezier-com-traj/common_solve_methods.hh
+++ b/include/bezier-com-traj/common_solve_methods.hh
@@ -14,19 +14,6 @@
namespace bezier_com_traj
{
-typedef Matrix63 matrix6_t;
-typedef Vector6 point6_t;
-typedef Matrix3 matrix3_t;
-typedef Vector3 point3_t;
-/**
- * @brief waypoint_t a waypoint is composed of a 6*3 matrix that depend
- * on the variable x, and of a 6d vector independent of x, such that
- * each control point of the target bezier curve is given by pi = wix * x + wis
- */
-typedef std::pair<matrix6_t, point6_t> waypoint6_t;
-typedef std::pair<matrix3_t, point3_t> waypoint3_t;
-typedef const Eigen::Ref<const point_t>& point_t_tC;
-typedef spline::bezier_curve <double, double, 6, true, point6_t> bezier6_t;
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
template<typename T> T initwp();
diff --git a/include/bezier-com-traj/data.hh b/include/bezier-com-traj/data.hh
index bd887eaa2951c381e82906bcac3a7d34a19f18dc..1b6affde8d6207817abaac350481035ace36ea0e 100644
--- a/include/bezier-com-traj/data.hh
+++ b/include/bezier-com-traj/data.hh
@@ -34,6 +34,20 @@ namespace bezier_com_traj
typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
+
+ typedef Matrix63 matrix6_t;
+ typedef Vector6 point6_t;
+ typedef Matrix3 matrix3_t;
+ typedef Vector3 point3_t;
+ /**
+ * @brief waypoint_t a waypoint is composed of a 6*3 matrix that depend
+ * on the variable x, and of a 6d vector independent of x, such that
+ * each control point of the target bezier curve is given by pi = wix * x + wis
+ */
+ typedef std::pair<matrix6_t, point6_t> waypoint6_t;
+ typedef std::pair<matrix3_t, point3_t> waypoint3_t;
+
+
struct BEZIER_COM_TRAJ_DLLAPI ContactData
{
ContactData()
@@ -51,6 +65,56 @@ namespace bezier_com_traj
VectorX ang_;
};
+ struct BEZIER_COM_TRAJ_DLLAPI Constraints
+ {
+
+ Constraints()
+ : c0_(true)
+ , dc0_(true)
+ , ddc0_(false)
+ , ddc1_(false)
+ , dc1_(true)
+ , c1_(true)
+ , 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)
+ , constraintAcceleration_(true)
+ , maxAcceleration_(5.)
+ ,reduce_h_(1e-4)
+ {
+ 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.");
+ }
+
+ ~Constraints(){}
+
+ bool c0_;
+ bool dc0_;
+ bool ddc0_;
+ bool ddc1_;
+ bool dc1_;
+ bool c1_;
+ bool constraintAcceleration_;
+ double maxAcceleration_;
+ double reduce_h_;
+
+ };
+
+
struct BEZIER_COM_TRAJ_DLLAPI ProblemData
{
ProblemData()
@@ -66,10 +130,13 @@ namespace bezier_com_traj
Vector3 c0_,dc0_,ddc0_,c1_,dc1_,ddc1_;
Vector3 l0_;
bool useAngularMomentum_;
+ Constraints constraints_;
};
typedef Eigen::Vector3d point_t;
+ typedef const Eigen::Ref<const point_t>& point_t_tC;
typedef spline::bezier_curve <double, double, 3, true, point_t > bezier_t;
+ typedef spline::bezier_curve <double, double, 6, true, point6_t> bezier6_t;
struct BEZIER_COM_TRAJ_DLLAPI ResultData
{
ResultData():
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
new file mode 100644
index 0000000000000000000000000000000000000000..8fc92f11d47cd18d50f474277785910517f9c185
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -0,0 +1,122 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_c1{
+typedef waypoint3_t waypoint_t;
+typedef std::pair<double,point3_t> coefs_t;
+
+
+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)
+ * @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;
+ // equation found with sympy
+ wp.first = -3.0*t3 + 3.0*t2;
+ wp.second = -1.0*pi[0]*t3 + 3.0*pi[0]*t2 - 3.0*pi[0]*t + 1.0*pi[0] + 3.0*pi[1]*t3 - 6.0*pi[1]*t2 + 3.0*pi[1]*t + 1.0*pi[3]*t3;
+ // 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);
+ // equation found with sympy
+ wp.first = (-18.0*t + 6.0)*alpha;
+ wp.second = (-6.0*pi[0]*t + 6.0*pi[0] + 18.0*pi[1]*t - 12.0*pi[1] + 6.0*pi[3]*t)*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 (degree 4, 4 constant waypoint and one free (p2))
+ // first, compute the constant waypoints that only depend on pData :
+ int n = 3;
+ std::vector<point_t> pi;
+ pi.push_back(pData.c0_); //p0
+ pi.push_back((pData.dc0_ * T / n )+ pData.c0_); // p1
+ pi.push_back(point_t::Zero()); // p2 = x
+ pi.push_back(pData.c1_); // p3
+
+ 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.first.block<3,3>(0,0) = 6*alpha*Matrix3::Identity();
+ w0.first.block<3,3>(3,0) = 6.0*Cpi[0]*alpha;
+ w0.second.head<3>() = (6*pi[0] - 12*pi[1])*alpha;
+ w0.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[0] - 12.0*Cpi[0]*pi[1])*alpha;
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(3,0) = 1.0*(-6.0*Cpi[0] + 6.0*Cpi[1])*alpha;
+ w1.second.head<3>() = 1.0*(4.0*pi[0] - 6.0*pi[1] + 2.0*pi[3])*alpha;
+ w1.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[1] + 2.0*Cpi[0]*pi[3])*alpha;
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = -6.0*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = 1.0*(1.0*Cg*T2 - 6.0*Cpi[1])*alpha;
+ w2.second.head<3>() = 1.0*(2.0*pi[0] + 4.0*pi[3])*alpha;
+ w2.second.tail<3>() = 1.0*(-2.0*Cpi[0]*pi[3] + 6.0*Cpi[1]*pi[3])*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -12*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = -12.0*Cpi[3]*alpha;
+ w3.second.head<3>() = (6*pi[1] + 6*pi[3])*alpha;
+ w3.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[3] - 6.0*Cpi[1]*pi[3])*alpha;
+ wps.push_back(w3);
+ return wps;
+}
+
+
+coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ // equation found with sympy
+ v.first = -3./T;
+ v.second = 3.* pData.c1_ / T;
+ return v;
+}
+
+coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = -12./(T*T);
+ v.second = (-6.*pi[1] + 6.*pi[3])/ (T*T);
+ return v;
+}
+
+
+}
+}
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
new file mode 100644
index 0000000000000000000000000000000000000000..b8d5e4cc0032563a663b09b39179ae57d9d1f444
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -0,0 +1,139 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+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){
+ 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)
+ * @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 = ( 6.0*t4 - 12.0*t3 + 6.0*t2);
+ 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 - 4.0*pi[3]*t4 + 4.0*pi[3]*t3 + 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);
+ // equation found with sympy
+ wp.first = (72.0*t*t - 72.0*t + 12.0)*alpha;
+ wp.second = (12.0*pi[0]*t*t - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t*t + 72.0*pi[1]*t - 24.0*pi[1] - 48.0*pi[3]*t*t + 24.0*pi[3]*t + 12.0*pi[4]*t*t)*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 (degree 4, 4 constant waypoint and one free (p2))
+ // first, compute the constant waypoints that only depend on pData :
+ int 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(point_t::Zero()); // p2 = x
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
+ pi.push_back(pData.c1_); // p4
+ /* for(int i = 0 ; i < pi.size() ; ++i){
+ std::cout<<" p"<<i<<" = "<<pi[i].transpose()<<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.first.block<3,3>(0,0) = 12.*alpha*Matrix3::Identity();
+ w0.first.block<3,3>(3,0) = 12.*alpha*Cpi[0];
+ w0.second.head<3>() = (12.*pi[0] - 24.*pi[1])*alpha;
+ w0.second.tail<3>() = 1.0*Cg*pi[0] - (24.0*Cpi[0]*pi[1])*alpha;
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = -2.4*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) =(-12.0*Cpi[0] + 9.6*Cpi[1])*alpha;
+ w1.second.head<3>() = (7.2*pi[0] - 9.6*pi[1] + 4.8*pi[3])*alpha;
+ w1.second.tail<3>() = (0.2*Cg*T2*pi[0] + 0.8*Cg*T2*pi[1] + 4.8*Cpi[0]*pi[3])*alpha;
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = -9.6*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = (0.6*Cg*T2 - 9.6*Cpi[1])*alpha;
+ w2.second.head<3>() = (3.6*pi[0] + 4.8*pi[3] + 1.2*pi[4])*alpha;
+ w2.second.tail<3>() = (0.4*Cg*T2*pi[1] - 4.8*Cpi[0]*pi[3] + 1.2*Cpi[0]*pi[4] + 9.6*Cpi[1]*pi[3])*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -9.6*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = (0.6*Cg*T2 - 9.6*Cpi[3])*alpha;
+ w3.second.head<3>() = (1.2*pi[0] + 4.8*pi[1] + 3.6*pi[4])*alpha;
+ w3.second.tail<3>() = (0.4*Cg*T2*pi[3] - 1.2*Cpi[0]*pi[4] - 9.6*Cpi[1]*pi[3] + 4.8*Cpi[1]*pi[4])*alpha;
+ wps.push_back(w3);
+ waypoint6_t w4 = initwp<waypoint6_t>();
+ w4.first.block<3,3>(0,0) = -2.4*alpha*Matrix3::Identity();
+ w4.first.block<3,3>(3,0) =(9.6*Cpi[3] - 12.0*Cpi[4])*alpha;
+ w4.second.head<3>() = (4.8*pi[1] - 9.6*pi[3] + 7.2*pi[4])*alpha;
+ w4.second.tail<3>() = (0.8*Cg*T2*pi[3] + 0.2*Cg*T2*pi[4] - 4.8*Cpi[1]*pi[4])*alpha;
+ wps.push_back(w4);
+ waypoint6_t w5 = initwp<waypoint6_t>();
+ w5.first.block<3,3>(0,0) =12*alpha*Matrix3::Identity();
+ w5.first.block<3,3>(3,0) =12.0*Cpi[4]*alpha;
+ w5.second.head<3>() = (-24*pi[3] + 12*pi[4])*alpha;
+ w5.second.tail<3>() =(Cg*T2*pi[4] + 24.0*Cpi[3]*pi[4])*alpha;
+ wps.push_back(w5);
+ return wps;
+}
+
+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 = (-4.0*pi[3] + 4.0*pi[4])/ T;
+ return v;
+}
+
+coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = 12./(T*T);
+ v.second = (-24.0*pi[3] + 12.*pi[4])/ (T*T);
+ return v;
+}
+
+
+}
+}
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
new file mode 100644
index 0000000000000000000000000000000000000000..f789a4430b57f4f602f10ef7b55e9522abdfe71e
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -0,0 +1,139 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_ddc0_c1{
+
+typedef waypoint3_t waypoint_t;
+typedef std::pair<double,point3_t> coefs_t;
+
+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)
+/**
+ * @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;
+}
+
+
+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;
+}
+
+coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ // equation found with sympy
+ v.first = -4./T;
+ v.second = 4.* pData.c1_ / T;
+ return v;
+}
+
+coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = -24./(T*T);
+ v.second = (12.0*pi[2] + 12.*pi[4])/ (T*T);
+ return v;
+}
+
+
+}
+}
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
new file mode 100644
index 0000000000000000000000000000000000000000..8a3d8ddac53640eb773ee23134ed6a1f8df351ee
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -0,0 +1,159 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_ddc0_dc1_c1{
+
+typedef waypoint3_t waypoint_t;
+typedef std::pair<double,point3_t> coefs_t;
+
+
+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)
+///
+/**
+ * @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;
+}
+
+
+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;
+}
+
+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 = (-5.0*pi[4] + 5.0*pi[5])/ T;
+ return v;
+}
+
+coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = 20./(T*T);
+ v.second = (-40.0*pi[4] + 20.*pi[5])/ (T*T);
+ return v;
+}
+
+
+}
+}
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
new file mode 100644
index 0000000000000000000000000000000000000000..0e5ebe1ead5f813dbdd502d3c691945c7b38c8fa
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -0,0 +1,168 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_ddc0_ddc1_dc1_c1{
+
+typedef waypoint3_t waypoint_t;
+typedef std::pair<double,point3_t> coefs_t;
+
+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)
+///
+/**
+ * @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
+ */
+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;
+ 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;
+ // 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;
+ 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;
+ // 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 = 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
+ /*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>() = (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;
+}
+
+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;
+}
+
+coefs_t computeFinalAccelerationPoint(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 = (-30.0*pi[4] - 60.*pi[5] + 30.*pi[6])/ (T*T);
+ return v;
+}
+}
+
+}
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c9a2bf9f697a6dc7f39e8a81f67c5aad370722d6
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -0,0 +1,158 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
+#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>
+
+
+
+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
+ */
+
+
+/** @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{
+ std::cout<<"Current constraints set are not implemented"<<std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
+}
+
+/** @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{
+ std::cout<<"Current constraints set are not implemented"<<std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
+}
+
+/**
+ * @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::cout<<"Current constraints set are not implemented"<<std::endl;
+ throw std::runtime_error("Current constraints set are not implemented");
+ }
+}
+
+/**
+ * @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::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_))
+ return c0_dc0_dc1_c1::computeFinalAccelerationPoint(pData,T);
+ else if(c0_dc0_ddc0_c1::useThisConstraints(pData.constraints_))
+ return c0_dc0_ddc0_c1::computeFinalAccelerationPoint(pData,T);
+ else if(c0_dc0_ddc0_dc1_c1::useThisConstraints(pData.constraints_))
+ return c0_dc0_ddc0_dc1_c1::computeFinalAccelerationPoint(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_){
+ res.ddc1_ = pData.ddc1_;
+ }else{
+ coefs_t a = computeFinalAccelerationPoint(pData,T);
+ res.ddc1_ = a.first*res.x + a.second;
+ }
+}
+
+void computeFinalVelocity(const ProblemData& pData,double T,ResultDataCOMTraj& res){
+ if(pData.constraints_.dc1_)
+ res.dc1_ = pData.dc1_;
+ else{
+ coefs_t v = computeFinalVelocityPoint(pData,T);
+ res.dc1_ = v.first*res.x + v.second;
+ }
+}
+
+
+}
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index 210aab999f3e34094baa6ced0bef69609c31ebed..c118a590b0e419020b7edb012781777ead093031 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -7,32 +7,16 @@
#include <bezier-com-traj/common_solve_methods.hh>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
#include <limits>
-
+#include <bezier-com-traj/waypoints/waypoints_definition.hh>
#ifndef QHULL
#define QHULL 1
#endif
-#ifndef DDC0_CONSTRAINT
-#define DDC0_CONSTRAINT 0
-#endif
-#ifndef DDC1_CONSTRAINT
-#define DDC1_CONSTRAINT 0
-#endif
-#ifndef DC1_CONSTRAINT
-#define DC1_CONSTRAINT 1
-#endif
+
#ifndef USE_SLACK
#define USE_SLACK 0
#endif
-#ifndef CONSTRAINT_ACC
-#define CONSTRAINT_ACC 0
-#endif
-#ifndef MAX_ACC
-#define MAX_ACC 5
-#endif
-#ifndef REDUCE_h
-#define REDUCE_h 1e-4
-#endif
+
namespace bezier_com_traj
{
@@ -65,636 +49,11 @@ void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab,VectorX intPoint,cons
}
-#if (!DDC0_CONSTRAINT && !DC1_CONSTRAINT)
-
-/// ### 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)
- * @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;
- // equation found with sympy
- wp.first = -3.0*t3 + 3.0*t2;
- wp.second = -1.0*pi[0]*t3 + 3.0*pi[0]*t2 - 3.0*pi[0]*t + 1.0*pi[0] + 3.0*pi[1]*t3 - 6.0*pi[1]*t2 + 3.0*pi[1]*t + 1.0*pi[3]*t3;
- // 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);
- // equation found with sympy
- wp.first = (-18.0*t + 6.0)*alpha;
- wp.second = (-6.0*pi[0]*t + 6.0*pi[0] + 18.0*pi[1]*t - 12.0*pi[1] + 6.0*pi[3]*t)*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 (degree 4, 4 constant waypoint and one free (p2))
- // first, compute the constant waypoints that only depend on pData :
- int n = 3;
- std::vector<point_t> pi;
- pi.push_back(pData.c0_); //p0
- pi.push_back((pData.dc0_ * T / n )+ pData.c0_); // p1
- pi.push_back(point_t::Zero()); // p2 = x
- pi.push_back(pData.c1_); // p3
-
- 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.first.block<3,3>(0,0) = 6*alpha*Matrix3::Identity();
- w0.first.block<3,3>(3,0) = 6.0*Cpi[0]*alpha;
- w0.second.head<3>() = (6*pi[0] - 12*pi[1])*alpha;
- w0.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[0] - 12.0*Cpi[0]*pi[1])*alpha;
- wps.push_back(w0);
- waypoint6_t w1 = initwp<waypoint6_t>();
- w1.first.block<3,3>(3,0) = 1.0*(-6.0*Cpi[0] + 6.0*Cpi[1])*alpha;
- w1.second.head<3>() = 1.0*(4.0*pi[0] - 6.0*pi[1] + 2.0*pi[3])*alpha;
- w1.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[1] + 2.0*Cpi[0]*pi[3])*alpha;
- wps.push_back(w1);
- waypoint6_t w2 = initwp<waypoint6_t>();
- w2.first.block<3,3>(0,0) = -6.0*alpha*Matrix3::Identity();
- w2.first.block<3,3>(3,0) = 1.0*(1.0*Cg*T2 - 6.0*Cpi[1])*alpha;
- w2.second.head<3>() = 1.0*(2.0*pi[0] + 4.0*pi[3])*alpha;
- w2.second.tail<3>() = 1.0*(-2.0*Cpi[0]*pi[3] + 6.0*Cpi[1]*pi[3])*alpha;
- wps.push_back(w2);
- waypoint6_t w3 = initwp<waypoint6_t>();
- w3.first.block<3,3>(0,0) = -12*alpha*Matrix3::Identity();
- w3.first.block<3,3>(3,0) = -12.0*Cpi[3]*alpha;
- w3.second.head<3>() = (6*pi[1] + 6*pi[3])*alpha;
- w3.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[3] - 6.0*Cpi[1]*pi[3])*alpha;
- wps.push_back(w3);
- return wps;
-}
-
-
-coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
- coefs_t v;
- // equation found with sympy
- v.first = -3./T;
- v.second = 3.* pData.c1_ / T;
- return v;
-}
-
-
-#endif // degree 3 : c0 dc0 x c1
-
-#if (!DDC0_CONSTRAINT && DC1_CONSTRAINT)
-
-/// ### 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)
- * @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 = ( 6.0*t4 - 12.0*t3 + 6.0*t2);
- 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 - 4.0*pi[3]*t4 + 4.0*pi[3]*t3 + 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);
- // equation found with sympy
- wp.first = (72.0*t*t - 72.0*t + 12.0)*alpha;
- wp.second = (12.0*pi[0]*t*t - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t*t + 72.0*pi[1]*t - 24.0*pi[1] - 48.0*pi[3]*t*t + 24.0*pi[3]*t + 12.0*pi[4]*t*t)*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 (degree 4, 4 constant waypoint and one free (p2))
- // first, compute the constant waypoints that only depend on pData :
- int 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(point_t::Zero()); // p2 = x
- pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
- pi.push_back(pData.c1_); // p4
- /* for(int i = 0 ; i < pi.size() ; ++i){
- std::cout<<" p"<<i<<" = "<<pi[i].transpose()<<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.first.block<3,3>(0,0) = 12.*alpha*Matrix3::Identity();
- w0.first.block<3,3>(3,0) = 12.*alpha*Cpi[0];
- w0.second.head<3>() = (12.*pi[0] - 24.*pi[1])*alpha;
- w0.second.tail<3>() = 1.0*Cg*pi[0] - (24.0*Cpi[0]*pi[1])*alpha;
- wps.push_back(w0);
- waypoint6_t w1 = initwp<waypoint6_t>();
- w1.first.block<3,3>(0,0) = -2.4*alpha*Matrix3::Identity();
- w1.first.block<3,3>(3,0) =(-12.0*Cpi[0] + 9.6*Cpi[1])*alpha;
- w1.second.head<3>() = (7.2*pi[0] - 9.6*pi[1] + 4.8*pi[3])*alpha;
- w1.second.tail<3>() = (0.2*Cg*T2*pi[0] + 0.8*Cg*T2*pi[1] + 4.8*Cpi[0]*pi[3])*alpha;
- wps.push_back(w1);
- waypoint6_t w2 = initwp<waypoint6_t>();
- w2.first.block<3,3>(0,0) = -9.6*alpha*Matrix3::Identity();
- w2.first.block<3,3>(3,0) = (0.6*Cg*T2 - 9.6*Cpi[1])*alpha;
- w2.second.head<3>() = (3.6*pi[0] + 4.8*pi[3] + 1.2*pi[4])*alpha;
- w2.second.tail<3>() = (0.4*Cg*T2*pi[1] - 4.8*Cpi[0]*pi[3] + 1.2*Cpi[0]*pi[4] + 9.6*Cpi[1]*pi[3])*alpha;
- wps.push_back(w2);
- waypoint6_t w3 = initwp<waypoint6_t>();
- w3.first.block<3,3>(0,0) = -9.6*alpha*Matrix3::Identity();
- w3.first.block<3,3>(3,0) = (0.6*Cg*T2 - 9.6*Cpi[3])*alpha;
- w3.second.head<3>() = (1.2*pi[0] + 4.8*pi[1] + 3.6*pi[4])*alpha;
- w3.second.tail<3>() = (0.4*Cg*T2*pi[3] - 1.2*Cpi[0]*pi[4] - 9.6*Cpi[1]*pi[3] + 4.8*Cpi[1]*pi[4])*alpha;
- wps.push_back(w3);
- waypoint6_t w4 = initwp<waypoint6_t>();
- w4.first.block<3,3>(0,0) = -2.4*alpha*Matrix3::Identity();
- w4.first.block<3,3>(3,0) =(9.6*Cpi[3] - 12.0*Cpi[4])*alpha;
- w4.second.head<3>() = (4.8*pi[1] - 9.6*pi[3] + 7.2*pi[4])*alpha;
- w4.second.tail<3>() = (0.8*Cg*T2*pi[3] + 0.2*Cg*T2*pi[4] - 4.8*Cpi[1]*pi[4])*alpha;
- wps.push_back(w4);
- waypoint6_t w5 = initwp<waypoint6_t>();
- w5.first.block<3,3>(0,0) =12*alpha*Matrix3::Identity();
- w5.first.block<3,3>(3,0) =12.0*Cpi[4]*alpha;
- w5.second.head<3>() = (-24*pi[3] + 12*pi[4])*alpha;
- w5.second.tail<3>() =(Cg*T2*pi[4] + 24.0*Cpi[3]*pi[4])*alpha;
- wps.push_back(w5);
- return wps;
-}
-
-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 = (-4.0*pi[3] + 4.0*pi[4])/ T;
- return v;
-}
-
-#endif // deg 4 : constraints on c0 dc0 x dc1 c1
-
-
-
-#if (DDC0_CONSTRAINT && DC1_CONSTRAINT && !DDC1_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;
-}
-
-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<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;
-}
-
-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 = (-5.0*pi[4] + 5.0*pi[5])/ T;
- return v;
-}
-
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 20./(T*T);
- v.second = (-40.0*pi[4] + 20.*pi[5])/ (T*T);
- return v;
-}
-
-
-#endif // deg 5 : constraints on c0 dc0 ddc0 x dc1 c1
-
-#if (DDC0_CONSTRAINT && DC1_CONSTRAINT && DDC1_CONSTRAINT)
-/// ### 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
- */
-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;
- 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;
- // 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;
- 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;
- // 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 = 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
- /*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>() = (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;
-}
-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;
-}
-
-coefs_t computeFinalAccelerationPoint(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 = (-30.0*pi[4] - 60.*pi[5] + 30.*pi[6])/ (T*T);
- return v;
-}
-#endif // deg 6 : constraints on c0 dc0 ddc0 x ddc1 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;
-}
-
-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<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
-
-
-
-
-void computeFinalVelocity(const ProblemData& pData,double T,ResultDataCOMTraj& res){
- #if DC1_CONSTRAINT
- res.dc1_ = pData.dc1_;
- #else
- coefs_t v = computeFinalVelocityPoint(pData,T);
- res.dc1_ = v.first*res.x + v.second;
- #endif
-}
-
-
-void computeFinalAcceleration(const ProblemData& pData,double T,ResultDataCOMTraj& res){
- /*
- point_t p2,p4;
- p2 = (pData.ddc0_*T*T/(4*(3))) + (2.*pData.dc0_ *T / 4) + pData.c0_;
- p4 = pData.c1_;
- coefs_t a;
- a.first = -24/(T*T);
- a.second = 12*(p2 + p4)/(T*T);
- res.ddc1_ = a.first * res.x + a.second;
- */
- #if DDC0_CONSTRAINT && ! DC1_CONSTRAINT
- coefs_t a = computeFinalAccelerationPoint(pData,T);
- res.ddc1_ = a.first*res.x + a.second;
- #else
- res.ddc1_ = pData.ddc1_;
- #endif
-}
-
/**
* @brief computeDiscretizedTime build an array of discretized points in time, such that there is the same number of point in each phase. Doesn't contain t=0, is of size pointsPerPhase*phaseTimings.size()
* @param phaseTimings
@@ -730,7 +89,7 @@ std::vector<coefs_t> computeDiscretizedWaypoints(const ProblemData& pData,double
t = timeArray[i] / T;
if(t>1)
t=1.;
- wps.push_back(evaluateCurveAtTime(pi,t));
+ wps.push_back(evaluateCurveAtTime(pData,pi,t));
}
return wps;
}
@@ -768,7 +127,7 @@ std::vector<coefs_t> computeDiscretizedAccelerationWaypoints(const ProblemData&
t = timeArray[i] / T;
if(t>1)
t=1.;
- wps.push_back(evaluateAccelerationCurveAtTime(pi,T,t));
+ wps.push_back(evaluateAccelerationCurveAtTime(pData,pi,T,t));
}
return wps;
}
@@ -857,10 +216,12 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
//std::cout<<"total time : "<<t_total<<std::endl;
std::vector<double> timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
std::vector<coefs_t> wps_c = computeDiscretizedWaypoints(pData,t_total,timeArray);
- #if CONSTRAINT_ACC
- std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints(pData,t_total,timeArray);
- Vector3 acc_bounds = Vector3::Ones()*MAX_ACC;
- #endif
+ std::vector<coefs_t> wps_ddc;
+ Vector3 acc_bounds;
+ if(pData.constraints_.constraintAcceleration_){
+ wps_ddc = computeDiscretizedAccelerationWaypoints(pData,t_total,timeArray);
+ acc_bounds = Vector3::Ones()*pData.constraints_.maxAcceleration_;
+ }
std::vector<waypoint6_t> wps_w = computeDiscretizedWwaypoints(pData,t_total,timeArray);
//std::cout<<" number of discretized waypoints c: "<<wps_c.size()<<std::endl;
//std::cout<<" number of discretized waypoints w: "<<wps_w.size()<<std::endl;
@@ -896,9 +257,9 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
num_kin_ineq += pData.contacts_[i].kin_.rows() * numStepForPhase;
}
num_ineq = num_stab_ineq + num_kin_ineq;
- #if CONSTRAINT_ACC
- num_ineq += 2*3 *(wps_c.size()) ; // upper and lower bound on acceleration for each discretized waypoint (exept the first one)
- #endif
+ if(pData.constraints_.constraintAcceleration_){
+ num_ineq += 2*3 *(wps_c.size()) ; // upper and lower bound on acceleration for each discretized waypoint (exept the first one)
+ }
//std::cout<<"total of inequalities : "<<num_ineq<<std::endl;
// init constraints matrix :
MatrixXX A = MatrixXX::Zero(num_ineq,numCol); // 3 + 1 : because of the slack constraints
@@ -920,8 +281,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
H.rowwise().normalize();
int dimH = (int)(H.rows());
mH = phase.contactPhase_->m_mass * H;
- if(REDUCE_h > 0)
- h -= VectorX::Ones(h.rows())*REDUCE_h;
+ if(pData.constraints_.reduce_h_ > 0 )
+ h -= VectorX::Ones(h.rows())*pData.constraints_.reduce_h_;
// assign the Stability constraints for each discretized waypoints :
// we don't consider the first point, because it's independant of x.
@@ -945,8 +306,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
H.rowwise().normalize();
dimH = (int)(H.rows());
mH = phase.contactPhase_->m_mass * H;
- if(REDUCE_h > 0)
- h -= VectorX::Ones(h.rows())*REDUCE_h;
+ if(pData.constraints_.reduce_h_ > 0 )
+ h -= VectorX::Ones(h.rows())*pData.constraints_.reduce_h_;
// the current waypoint must have the constraints of both phases. So we add it again :
// TODO : filter for redunbdant constraints ...
// add stability constraints :
@@ -990,17 +351,15 @@ std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData,
}
}
- #if CONSTRAINT_ACC
- // assign the acceleration constraints for each discretized waypoints :
- for(int id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
- A.block(id_rows,0,3,3) = Matrix3::Identity() * wps_ddc[id_step].first; // upper
- b.segment(id_rows,3) = acc_bounds - wps_ddc[id_step].second;
- A.block(id_rows+3,0,3,3) = -Matrix3::Identity() * wps_ddc[id_step].first; // lower
- b.segment(id_rows+3,3) = acc_bounds + wps_ddc[id_step].second;
- id_rows += 6;
+ if(pData.constraints_.constraintAcceleration_) { // assign the acceleration constraints for each discretized waypoints :
+ for(int id_step = 0 ; id_step < timeArray.size() ; ++id_step ){
+ A.block(id_rows,0,3,3) = Matrix3::Identity() * wps_ddc[id_step].first; // upper
+ b.segment(id_rows,3) = acc_bounds - wps_ddc[id_step].second;
+ A.block(id_rows+3,0,3,3) = -Matrix3::Identity() * wps_ddc[id_step].first; // lower
+ b.segment(id_rows+3,3) = acc_bounds + wps_ddc[id_step].second;
+ id_rows += 6;
+ }
}
- #endif
-
//std::cout<<"id rows : "<<id_rows<<" ; total rows : "<<A.rows()<<std::endl;
assert(id_rows == (A.rows()) && "The constraints matrices were not fully filled.");
return std::make_pair(A,b);
@@ -1047,24 +406,35 @@ void computeBezierCurve(const ProblemData& pData, const double T, ResultDataCOMT
std::vector<Vector3> wps;
std::vector<Vector3> pi = computeConstantWaypoints(pData,T);
- size_t i = 2;
- wps.push_back(pi[0]);
- wps.push_back(pi[1]);
- #if DDC0_CONSTRAINT
+ size_t i = 0;
+ if(pData.constraints_.c0_){
wps.push_back(pi[i]);
i++;
- #endif
+ if(pData.constraints_.dc0_){
+ wps.push_back(pi[i]);
+ i++;
+ if(pData.constraints_.ddc0_){
+ wps.push_back(pi[i]);
+ i++;
+ }
+ }
+ }
wps.push_back(res.x);
i++;
- #if DDC1_CONSTRAINT
+ if(pData.constraints_.ddc1_){
+ assert(pData.constraints_.dc1_ && "You cannot constraint final acceleration if final velocity is not constrained.");
wps.push_back(pi[i]);
i++;
- #endif
- #if DC1_CONSTRAINT
+ }
+ if(pData.constraints_.dc1_){
+ assert(pData.constraints_.c1_ && "You cannot constraint final velocity if final position is not constrained.");
wps.push_back(pi[i]);
i++;
- #endif
- wps.push_back(pi[i]);
+ }
+ if(pData.constraints_.c1_){
+ wps.push_back(pi[i]);
+ i++;
+ }
res.c_of_t_ = bezier_t (wps.begin(), wps.end(),T);
}
@@ -1105,12 +475,11 @@ ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const
ResultDataCOMTraj res;
VectorX constraint_equivalence;
std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,pointsPerPhase,constraint_equivalence);
- #if DC1_CONSTRAINT
//computeCostMidPoint(pData,H,g);
- computeCostMinAcceleration(pData,Ts,pointsPerPhase,H,g);
- #else
- computeCostEndVelocity(pData,T,H,g);
- #endif
+ if(pData.constraints_.dc1_)
+ computeCostMinAcceleration(pData,Ts,pointsPerPhase,H,g);
+ else
+ computeCostEndVelocity(pData,T,H,g);
#if USE_SLACK
addSlackInCost(H,g);