diff --git a/include/bezier-com-traj/definitions.hh b/include/bezier-com-traj/definitions.hh
index 5ad093e9c04126c989a70ec6491bff668e57e7bf..1d65ca11e1af3b16d25c08811a158b055488cc40 100644
--- a/include/bezier-com-traj/definitions.hh
+++ b/include/bezier-com-traj/definitions.hh
@@ -16,6 +16,7 @@ namespace bezier_com_traj
typedef double value_type;
typedef Eigen::Matrix <value_type, 3, 3> Matrix3;
typedef Eigen::Matrix <value_type, 6, 3> Matrix63;
+typedef Eigen::Matrix <value_type, 3, 9> Matrix39;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3> MatrixX3;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic> MatrixXX;
typedef centroidal_dynamics::Vector3 Vector3;
@@ -52,8 +53,11 @@ typedef T_time::const_iterator CIT_time;
* 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<MatrixXX, VectorX> waypoint_t;
typedef std::pair<matrix6_t, point6_t> waypoint6_t;
typedef std::pair<matrix3_t, point3_t> waypoint3_t;
+typedef std::pair<Matrix39, point3_t> waypoint9_t;
+
typedef std::pair<double,point3_t> coefs_t;
} // end namespace bezier_com_traj
diff --git a/include/bezier-com-traj/solve_end_effector.hh b/include/bezier-com-traj/solve_end_effector.hh
index b6a9e529fe6c1aac19b39146e82f64ff3c62b211..2f6e5e3d718c92826a41000fc91776ee94deea9c 100644
--- a/include/bezier-com-traj/solve_end_effector.hh
+++ b/include/bezier-com-traj/solve_end_effector.hh
@@ -12,7 +12,6 @@ using namespace bezier_com_traj;
namespace bezier_com_traj
{
-typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
const int DIM_POINT=3;
const int NUM_DISCRETIZATION = 11;
diff --git a/include/bezier-com-traj/utils.hh b/include/bezier-com-traj/utils.hh
index 09ba48beded0bbd704245930ee15eb3382f04700..996f3004c24cd60b24520c2962e622443329f885 100644
--- a/include/bezier-com-traj/utils.hh
+++ b/include/bezier-com-traj/utils.hh
@@ -17,6 +17,7 @@
namespace bezier_com_traj
{
template<typename T> T initwp();
+waypoint_t initwp(const size_t rows, const size_t cols);
/**
* @brief Compute the Bernstein polynoms for a given degree
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
index cf6685b37a79be71810a6e8f305ad34f08894bb1..edfca97109df59846edd1dacc96c1753d9989958 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
@@ -12,7 +12,8 @@ namespace bezier_com_traj{
namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1{
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK;
-
+static const size_t DIM_VAR = 3;
+static const size_t DIM_POINT = 3;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK (DEGREE = 8)
///
/**
@@ -170,87 +171,88 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
return wps;
}
-std::vector<waypoint3_t> computeVelocityWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
+std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
if(pi.size() == 0)
pi = computeConstantWaypoints(pData,T);
- std::vector<waypoint3_t> wps;
+ std::vector<waypoint_t> wps;
assert(pi.size() == 9);
double alpha = 1. / (T);
- waypoint3_t w = initwp<waypoint3_t>();
+ waypoint_t w = initwp(DIM_POINT,DIM_VAR);
// assign w0:
w.second= alpha*8*(-pi[0]+pi[1]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w1:
w.second = alpha*8*(-pi[1]+pi[2]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w2:
w.second = alpha*8*(-pi[2]+pi[3]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w3:
w.first = 8*alpha*Matrix3::Identity();
w.second = alpha*-8*pi[3];
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w4:
w.first = -8*alpha*Matrix3::Identity();
w.second = alpha*8*pi[5];
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w5:
w.second=alpha*8*(-pi[5]+pi[6]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w6:
w.second=alpha*8*(-pi[6]+pi[7]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w7:
w.second=alpha*8*(-pi[7]+pi[8]);
wps.push_back(w);
return wps;
}
-std::vector<waypoint3_t> computeAccelerationWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
+std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
if(pi.size() == 0)
pi = computeConstantWaypoints(pData,T);
- std::vector<waypoint3_t> wps;
+ std::vector<waypoint_t> wps;
assert(pi.size() == 9);
double alpha = 1. / (T*T);
- waypoint3_t w = initwp<waypoint3_t>();
+ waypoint_t w = initwp(DIM_POINT,DIM_VAR);
+
// assign w0:
w.second= 56*alpha*(pi[0] - 2*pi[1] + pi[2]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w1:
w.second= 56*alpha*(pi[1] - 2*pi[2] + pi[3]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w2:
w.first = 56*alpha*Matrix3::Identity();
w.second = (56*pi[2] - 112*pi[3])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w3:
w.first = -112*alpha*Matrix3::Identity();
w.second = (56*pi[3] +56*pi[8])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w4:
w.first = 56*alpha*Matrix3::Identity();
w.second = (-112*pi[5] + 56*pi[6])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w5:
w.second=56*alpha*(pi[5]-2*pi[6]+pi[7]);
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w5:
w.second=56*alpha*(pi[6]-2*pi[7]+pi[8]);
wps.push_back(w);
@@ -258,40 +260,41 @@ std::vector<waypoint3_t> computeAccelerationWaypoints(const ProblemData& pData,c
}
-std::vector<waypoint3_t> computeJerkWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
+std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
if(pi.size() == 0)
pi = computeConstantWaypoints(pData,T);
- std::vector<waypoint3_t> wps;
+ std::vector<waypoint_t> wps;
assert(pi.size() == 9);
double alpha = 1. / (T*T*T);
- waypoint3_t w = initwp<waypoint3_t>();
+ waypoint_t w = initwp(DIM_POINT,DIM_VAR);
+
// assign w0:
w.second= 336*(-pi[0] +3*pi[1] - 3*pi[2] + pi[3])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w1:
w.first = 336*alpha*Matrix3::Identity();
w.second= 336*(-pi[1] + 3*pi[2] - 3*pi[3])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w2:
w.first = -3*336*alpha*Matrix3::Identity();
w.second = 336*(-pi[2] + 3*pi[3] + pi[5])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w3:
w.first = 3*336*alpha*Matrix3::Identity();
w.second = 336*(-pi[3] - 3*pi[5] + pi[6])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w4:
w.first = -336*alpha*Matrix3::Identity();
w.second = 336*(3*pi[5] - 3*pi[6] + pi[7])*alpha;
wps.push_back(w);
- w = initwp<waypoint3_t>();
+ w = initwp(DIM_POINT,DIM_VAR);
// assign w5:
w.second= 336*(-pi[5] + 3*pi[6] - 3*pi[7] + pi[8])*alpha;
wps.push_back(w);
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
new file mode 100644
index 0000000000000000000000000000000000000000..dbf66fbfdd41c7fb94f1810fff2b05619babee8b
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
@@ -0,0 +1,308 @@
+#ifndef BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_X3_J1_DDC1_DC1_C1_HH
+#define BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_X3_J1_DDC1_DC1_C1_HH
+
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1{
+
+static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK | THREE_FREE_VAR;
+static const size_t DIM_VAR = 9;
+static const size_t DIM_POINT = 3;
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK AND 3 variables in the middle (DEGREE = 10)
+///
+/**
+ * @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 pi[8] pi[1] pi[2] pi[3] x0 x1 x2 pi[4] pi[5] pi[6] pi[7]
+ * @param t param (normalized !)
+ * @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
+ */
+//TODO
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
+ coefs_t wp;
+ const double t2 = t*t;
+ const double t3 = t2*t;
+ const double t4 = t3*t;
+ const double t5 = t4*t;
+ const double t6 = t5*t;
+ const double t7 = t6*t;
+ const double t8 = t7*t;
+ // equation found with sympy
+ wp.first = 70.0*t8 - 280.0*t7 + 420.0*t6 - 280.0*t5 + 70.0*t4;
+ wp.second = 1.0*pi[8]*t8 - 8.0*pi[8]*t7 + 28.0*pi[8]*t6 - 56.0*pi[8]*t5 + 70.0*pi[8]*t4 - 56.0*pi[8]*t3 + 28.0*pi[8]*t2 - 8.0*pi[8]*t + 1.0*pi[8] - 8.0*pi[1]*t8 + 56.0*pi[1]*t7 - 168.0*pi[1]*t6 + 280.0*pi[1]*t5 - 280.0*pi[1]*t4 + 168.0*pi[1]*t3 - 56.0*pi[1]*t2 + 8.0*pi[1]*t + 28.0*pi[2]*t8 - 168.0*pi[2]*t7 + 420.0*pi[2]*t6 - 560.0*pi[2]*t5 + 420.0*pi[2]*t4 - 168.0*pi[2]*t3 + 28.0*pi[2]*t2 - 56.0*pi[3]*pow(t,8 )+ 280.0*pi[3]*t7 - 560.0*pi[3]*t6 + 560.0*pi[3]*t5 - 280.0*pi[3]*t4 + 56.0*pi[3]*t3 - 56.0*pi[5]*t8 + 168.0*pi[5]*t7 - 168.0*pi[5]*t6 + 56.0*pi[5]*pow(t,5 )+ 28.0*pi[6]*t8 - 56.0*pi[6]*t7 + 28.0*pi[6]*t6 - 8.0*pi[7]*t8 + 8.0*pi[7]*t7 + 1.0*pi[8]*t8;
+ return wp;
+}
+
+//TODO
+inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi,double T,double t){
+ coefs_t wp;
+ const double alpha = 1./(T);
+ const double t2 = t*t;
+ const double t3 = t2*t;
+ const double t4 = t3*t;
+ const double t5 = t4*t;
+ const double t6 = t5*t;
+ const double t7 = t6*t;
+ // equation found with sympy
+ wp.first = (560.0*t7 - 1960.0*t6 + 2520.0*t5 - 1400.0*t4 + 280.0*t3)*alpha;
+ wp.second = (8.0*pi[8]*t7 - 56.0*pi[8]*t6 + 168.0*pi[8]*t5 - 280.0*pi[8]*t4 + 280.0*pi[8]*t3 - 168.0*pi[8]*t2 + 56.0*pi[8]*t - 8.0*pi[8] - 64.0*pi[1]*t7 + 392.0*pi[1]*t6 - 1008.0*pi[1]*t5 + 1400.0*pi[1]*t4 - 1120.0*pi[1]*t3 + 504.0*pi[1]*t2 - 112.0*pi[1]*t + 8.0*pi[1] + 224.0*pi[2]*t7 - 1176.0*pi[2]*t6 + 2520.0*pi[2]*t5 - 2800.0*pi[2]*t4 + 1680.0*pi[2]*t3 - 504.0*pi[2]*t2 + 56.0*pi[2]*t - 448.0*pi[3]*t7 + 1960.0*pi[3]*t6 - 3360.0*pi[3]*t5 + 2800.0*pi[3]*t4 - 1120.0*pi[3]*t3 + 168.0*pi[3]*t2 - 448.0*pi[5]*t7 + 1176.0*pi[5]*t6 - 1008.0*pi[5]*t5 + 280.0*pi[5]*t4 + 224.0*pi[6]*t7 - 392.0*pi[6]*t6 + 168.0*pi[6]*t5 - 64.0*pi[7]*t7 + 56.0*pi[7]*t6 + 8.0*pi[8]*t7)*alpha;
+ return wp;
+}
+
+
+//TODO
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
+ coefs_t wp;
+ const double alpha = 1./(T*T);
+ const double t2 = t*t;
+ const double t3 = t2*t;
+ const double t4 = t3*t;
+ const double t5 = t4*t;
+ const double t6 = t5*t;
+ // equation found with sympy
+ wp.first = ((3920.0*t6 - 11760.0*t5 + 12600.0*t4 - 5600.0*t3 + 840.0*t2))*alpha;
+ wp.second = (56.0*pi[8]*t6 - 336.0*pi[8]*t5 + 840.0*pi[8]*t4 - 1120.0*pi[8]*t3 + 840.0*pi[8]*t2 - 336.0*pi[8]*t + 56.0*pi[8] - 448.0*pi[1]*t6 + 2352.0*pi[1]*t5 - 5040.0*pi[1]*t4 + 5600.0*pi[1]*t3 - 3360.0*pi[1]*t2 + 1008.0*pi[1]*t - 112.0*pi[1] + 1568.0*pi[2]*t6 - 7056.0*pi[2]*t5 + 12600.0*pi[2]*t4 - 11200.0*pi[2]*t3 + 5040.0*pi[2]*t2 - 1008.0*pi[2]*t + 56.0*pi[2] - 3136.0*pi[3]*t6 + 11760.0*pi[3]*t5 - 16800.0*pi[3]*t4 + 11200.0*pi[3]*t3 - 3360.0*pi[3]*t2+ 336.0*pi[3]*t - 3136.0*pi[5]*t6 + 7056.0*pi[5]*t5 - 5040.0*pi[5]*t4 + 1120.0*pi[5]*t3 + 1568.0*pi[6]*t6 - 2352.0*pi[6]*t5 + 840.0*pi[6]*t4 - 448.0*pi[7]*t6 + 336.0*pi[7]*t5 + 56.0*pi[8]*t6)*alpha;
+ return wp;
+}
+
+//TODO
+inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi,double T,double t){
+ coefs_t wp;
+ const double alpha = 1./(T*T*T);
+ const double t2 = t*t;
+ const double t3 = t2*t;
+ const double t4 = t3*t;
+ const double t5 = t4*t;
+ // equation found with sympy
+ wp.first = (23520.0*t5 - 58800.0*t4 + 50400.0*t3 - 16800.0*t2 + 1680.0*t)*alpha;
+ wp.second = 1.0*(336.0*pi[0]*t5 - 1680.0*pi[0]*t4 + 3360.0*pi[0]*t3 - 3360.0*pi[0]*t2 + 1680.0*pi[0]*t - 336.0*pi[0] - 2688.0*pi[1]*t5 + 11760.0*pi[1]*t4 - 20160.0*pi[1]*t3 + 16800.0*pi[1]*t2 - 6720.0*pi[1]*t + 1008.0*pi[1] + 9408.0*pi[2]*t5 - 35280.0*pi[2]*t4 + 50400.0*pi[2]*t3 - 33600.0*pi[2]*t2 + 10080.0*pi[2]*t - 1008.0*pi[2] - 18816.0*pi[3]*t5 + 58800.0*pi[3]*t4 - 67200.0*pi[3]*t3 + 33600.0*pi[3]*t2 - 6720.0*pi[3]*t + 336.0*pi[3] - 18816.0*pi[5]*t5 + 35280.0*pi[5]*t4 - 20160.0*pi[5]*t3 + 3360.0*pi[5]*t2 + 9408.0*pi[6]*t5 - 11760.0*pi[6]*t4 + 3360.0*pi[6]*t3 - 2688.0*pi[7]*t5 + 1680.0*pi[7]*t4 + 336.0*pi[8]*t5)*alpha;
+ return wp;
+}
+
+
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+ // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
+ // first, compute the constant waypoints that only depend on pData :
+ double n = 10.;
+ std::vector<point_t> pi;
+ pi.push_back(pData.c0_);
+ pi.push_back((pData.dc0_ * T / n )+ pData.c0_);
+ pi.push_back((pData.ddc0_*T*T/(n*(n-1))) + (2*pData.dc0_ *T / n) + pData.c0_); // * T because derivation make a T appear
+ pi.push_back((pData.j0_*T*T*T/(n*(n-1)*(n-2)))+ (3*pData.ddc0_*T*T/(n*(n-1))) + (3*pData.dc0_ *T / n) + pData.c0_);
+ pi.push_back(point_t::Zero());
+ pi.push_back(point_t::Zero());
+ pi.push_back(point_t::Zero());
+ pi.push_back((-pData.j1_*T*T*T/(n*(n-1)*(n-2))) + (3*pData.ddc1_ *T*T / (n*(n-1))) - (3 * pData.dc1_ *T / n) + pData.c1_ ); // * T ??
+ pi.push_back((pData.ddc1_ *T*T / (n*(n-1))) - (2 * pData.dc1_ *T / n) + pData.c1_ ); // * T ??
+ pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
+ pi.push_back(pData.c1_);
+ return pi;
+}
+
+//TODO
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+ std::vector<waypoint6_t> wps;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ std::vector<Matrix3> Cpi;
+ for(std::size_t i = 0 ; i < pi.size() ; ++i){
+ Cpi.push_back(skew(pi[i]));
+ }
+ const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
+ const Matrix3 Cg = skew( g);
+ const double T2 = T*T;
+ const double alpha = 1/(T2);
+ std::cout<<"NOT IMPLEMENTED YET"<<std::endl;
+ return wps;
+}
+
+std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
+ if(pi.size() == 0)
+ pi = computeConstantWaypoints(pData,T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 11);
+
+ double alpha = 1. / (T);
+ waypoint_t w = initwp(DIM_POINT,DIM_VAR);
+
+ // assign w0:
+ w.second= alpha*10*(-pi[0] + pi[1]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w1:
+ w.second = alpha*10*(-pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w2:
+ w.second = alpha*10*(-pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w3:
+ w.first.block<3,3>(0,0) = 10*alpha*Matrix3::Identity(); // x0
+ w.second = alpha*10*(-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w4:
+ w.first.block<3,3>(0,0) = -10*alpha*Matrix3::Identity(); // x0
+ w.first.block<3,3>(0,3) = 10*alpha*Matrix3::Identity(); // x1
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w5:
+ w.first.block<3,3>(0,3) = -10*alpha*Matrix3::Identity(); // x1
+ w.first.block<3,3>(0,6) = 10*alpha*Matrix3::Identity(); // x2
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w6:
+ w.first.block<3,3>(0,6) = -10*alpha*Matrix3::Identity(); // x2
+ w.second=alpha*10*pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w7:
+ w.second=alpha*10*(-pi[7] + pi[8]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w8:
+ w.second=alpha*10*(-pi[8] + pi[9]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w9:
+ w.second=alpha*10*(pi[10] - pi[9]);
+ wps.push_back(w);
+ return wps;
+}
+
+std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
+ if(pi.size() == 0)
+ pi = computeConstantWaypoints(pData,T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 11);
+ double alpha = 1. / (T*T);
+
+ waypoint_t w = initwp(DIM_POINT,DIM_VAR);
+
+ // assign w0:
+ w.second= alpha*90*(pi[0] - 2*pi[1] + pi[2]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w1:
+ w.second= alpha*90*(pi[1] - 2*pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w2:
+ w.first.block<3,3>(0,0) = 90*alpha*Matrix3::Identity();//x0
+ w.second = alpha*90*(pi[2]-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w3:
+ w.first.block<3,3>(0,0) = -180*alpha*Matrix3::Identity(); // x0
+ w.first.block<3,3>(0,3) = 90*alpha*Matrix3::Identity(); // x1
+ w.second = alpha*90*pi[3];
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w4:
+ w.first.block<3,3>(0,0) = 90*alpha*Matrix3::Identity(); // x0
+ w.first.block<3,3>(0,3) = -180*alpha*Matrix3::Identity(); // x1
+ w.first.block<3,3>(0,6) = 90*alpha*Matrix3::Identity(); // x2
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w5:
+ w.first.block<3,3>(0,3) = 90*alpha*Matrix3::Identity(); // x1
+ w.first.block<3,3>(0,6) = -180*alpha*Matrix3::Identity(); // x2
+ w.second=alpha*90*pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w6:
+ w.first.block<3,3>(0,6) = 90*alpha*Matrix3::Identity(); // x2
+ w.second=alpha*90*(-2*pi[7] + pi[8]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w7:
+ w.second=alpha*90*(pi[7] - 2*pi[8] + pi[9]);
+ wps.push_back(w);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w8:
+ w.second=alpha*90*(pi[10] + pi[8] - 2*pi[9]);
+ wps.push_back(w);
+ return wps;
+}
+
+std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData,const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()){
+ if(pi.size() == 0)
+ pi = computeConstantWaypoints(pData,T);
+
+ std::vector<waypoint_t> wps;
+ assert(pi.size() == 11);
+
+ double alpha = 1. / (T*T*T);
+
+ waypoint_t w = initwp(DIM_POINT,DIM_VAR);
+
+ // assign w0:
+ w.second= alpha*720*(-pi[0] + 3*pi[1] - 3*pi[2] + pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w1:
+ w.first.block<3,3>(0,0) = 720*alpha*Matrix3::Identity(); //x0
+ w.second= alpha*720*(-pi[1] + 3*pi[2] - 3*pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w2:
+ w.first.block<3,3>(0,0) = 720*-3*alpha*Matrix3::Identity(); // x0
+ w.first.block<3,3>(0,3) = 720*alpha*Matrix3::Identity(); //x1
+ w.second = alpha*720*(-pi[2] + 3*pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w3:
+ w.first.block<3,3>(0,0) = 720*3*alpha*Matrix3::Identity(); // x0
+ w.first.block<3,3>(0,3) = 720*-3*alpha*Matrix3::Identity(); //x1
+ w.first.block<3,3>(0,6) = 720*alpha*Matrix3::Identity(); // x2
+ w.second = alpha*720*(-pi[3]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w4:
+ w.first.block<3,3>(0,0) = -720*alpha*Matrix3::Identity(); // x0
+ w.first.block<3,3>(0,3) = 720*3*alpha*Matrix3::Identity(); //x1
+ w.first.block<3,3>(0,6) = 720*-3*alpha*Matrix3::Identity(); // x2
+ w.second = alpha*720*pi[7];
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w5:
+ w.first.block<3,3>(0,3) = -720*alpha*Matrix3::Identity(); //x1
+ w.first.block<3,3>(0,6) = 720*3*alpha*Matrix3::Identity(); // x2
+ w.second=alpha* 720*(-3*pi[7] + pi[8]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w5:
+ w.first.block<3,3>(0,6) = -720*alpha*Matrix3::Identity(); // x2
+ w.second=alpha*720*(3*pi[7] - 3*pi[8] + pi[9]);
+ wps.push_back(w);
+ w = initwp(DIM_POINT,DIM_VAR);
+ // assign w6:
+ w.second=alpha*720*(pi[10] - pi[7] + 3*pi[8] - 3*pi[9]);
+ wps.push_back(w);
+ return wps;
+}
+
+//TODO
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = (-6.0*pi[5] + 6.0*pi[6])/ T;
+ return v;
+}
+
+}
+
+}
+
+
+#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
index 146d8a620133e85bb78926246eeb26c7c95ff6d9..6fb633431d7bad147b2082c54fb90410460bf4ba 100644
--- a/include/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -65,7 +65,7 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
* @param T
* @return
*/
-std::vector<waypoint3_t> computeVelocityWaypoints(const ProblemData& pData,const double T,std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData,const double T,std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
@@ -75,7 +75,7 @@ std::vector<waypoint3_t> computeVelocityWaypoints(const ProblemData& pData,const
* @param T
* @return
*/
-std::vector<waypoint3_t> computeAccelerationWaypoints(const ProblemData& pData,const double T,std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData,const double T,std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
* @brief computeWwaypoints compute the constant waypoints of dddc(t)
@@ -84,7 +84,7 @@ std::vector<waypoint3_t> computeAccelerationWaypoints(const ProblemData& pData,c
* @param T
* @return
*/
-std::vector<waypoint3_t> computeJerkWaypoints(const ProblemData& pData,const double T,std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
+std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData,const double T,std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
index d0d3513d968d80bc71bdfa20a3b9c9801c11c6ae..35a2b0b9f2011e96f4afdb55d91640c2b09c3620 100644
--- a/src/computeCOMTraj.cpp
+++ b/src/computeCOMTraj.cpp
@@ -22,7 +22,6 @@ const int numCol = 3;
namespace bezier_com_traj
{
-typedef waypoint3_t waypoint_t;
typedef std::pair<double,point3_t> coefs_t;
std::vector<waypoint6_t> computeDiscretizedWwaypoints(const ProblemData& pData,double T, const T_time& timeArray)
diff --git a/src/solve_0_step.cpp b/src/solve_0_step.cpp
index 73af740c740db860390f267ec9af2a61279f507d..e984d48ed99a7582a31270a7971d3647346b47a7 100644
--- a/src/solve_0_step.cpp
+++ b/src/solve_0_step.cpp
@@ -8,12 +8,11 @@
#include <bezier-com-traj/common_solve_methods.hh>
using namespace bezier_com_traj;
-typedef waypoint6_t waypoint_t;
namespace bezier_com_traj
{
-waypoint_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, const Matrix3& /*p1X*/, const Matrix3& /*gX*/, const double alpha)
+waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, const Matrix3& /*p1X*/, const Matrix3& /*gX*/, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3,3>(0,0) = 6*alpha* Matrix3::Identity();
w.first.block<3,3>(3,0) = 6*alpha*p0X;
w.second.head(3) = 6*alpha*(p0 - 2*p1);
@@ -21,9 +20,9 @@ waypoint_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, co
return w;
}
-waypoint_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& gX, const double alpha)
+waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& gX, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3,3>(0,0) = 3*alpha*Matrix3::Identity();
w.first.block<3,3>(3,0) = skew(1.5 * (3*p1 - p0))*alpha;
w.second.head(3) = 1.5 *alpha* (3*p0 - 5*p1);
@@ -31,9 +30,9 @@ waypoint_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*p
return w;
}
-waypoint_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& gX, const double alpha)
+waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& gX, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
// w.first.block<3,3>(0,0) = 0;
w.first.block<3,3>(3,0) = skew(0.5*g - 3*alpha* p0 + 3*alpha*p1);
w.second.head(3) = 3*alpha*(p0 - p1);
@@ -41,9 +40,9 @@ waypoint_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/
return w;
}
-waypoint_t w3 (point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& /*gX*/, const double alpha)
+waypoint6_t w3 (point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& /*gX*/, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3,3>(0,0) = -3*alpha*Matrix3::Identity();
w.first.block<3,3>(3,0) = skew(g - 1.5 *alpha* (p1 + p0));
w.second.head(3) = 1.5*alpha * (p1 + p0);
@@ -51,9 +50,9 @@ waypoint_t w3 (point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*
return w;
}
-waypoint_t w4 (point_t_tC /*p0*/, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& /*gX*/, const double alpha)
+waypoint6_t w4 (point_t_tC /*p0*/, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/, const Matrix3& /*gX*/, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3,3>(0,0) = -6*alpha * Matrix3::Identity();
w.first.block<3,3>(3,0) = skew(g - 6*alpha* p1);
w.second.head(3) = 6*alpha*p1;
@@ -61,9 +60,9 @@ waypoint_t w4 (point_t_tC /*p0*/, point_t_tC p1, point_t_tC g, const Matrix3& /*
return w;
}
-waypoint_t u0 (point_t_tC l0, const double alpha)
+waypoint6_t u0 (point_t_tC l0, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
//w.first.block<3,3>(0,0) = 0;
w.first.block<3,3>(3,0) = 3*alpha * Matrix3::Identity();
//w.second.head(3) = 0;
@@ -71,9 +70,9 @@ waypoint_t u0 (point_t_tC l0, const double alpha)
return w;
}
-waypoint_t u1 (point_t_tC l0, const double alpha)
+waypoint6_t u1 (point_t_tC l0, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
//w.first.block<3,3>(0,0) = 0;
//w.first.block<3,3>(3,0) = 0;
//w.second.head(3) = 0;
@@ -81,9 +80,9 @@ waypoint_t u1 (point_t_tC l0, const double alpha)
return w;
}
-waypoint_t u2 (point_t_tC l0, const double alpha)
+waypoint6_t u2 (point_t_tC l0, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
//w.first.block<3,3>(0,0) = 0;
w.first.block<3,3>(3,0) = -1.5*alpha * Matrix3::Identity();
//w.second.head(3) = 0;
@@ -91,9 +90,9 @@ waypoint_t u2 (point_t_tC l0, const double alpha)
return w;
}
-waypoint_t u3 (point_t_tC /*l0*/, const double alpha)
+waypoint6_t u3 (point_t_tC /*l0*/, const double alpha)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3,3>(3,0) = -1.5*alpha * Matrix3::Identity();
//w.second.head(3) = 0;
//w.second.tail(3) = 0.;
@@ -101,9 +100,9 @@ waypoint_t u3 (point_t_tC /*l0*/, const double alpha)
}
-waypoint_t u4 (point_t_tC /*l0*/, const double /*alpha*/)
+waypoint6_t u4 (point_t_tC /*l0*/, const double /*alpha*/)
{
- waypoint_t w = initwp<waypoint_t>();
+ waypoint6_t w = initwp<waypoint6_t>();
//w.first.block<3,3>(0,0) = 0;
//w.first.block<3,3>(3,0) = 0;
//w.second.head(3) = 0;
@@ -117,7 +116,7 @@ int computeNumSteps(const double T, const double timeStep)
return timeStep > 0. ? int(T / timeStep) : -1;
}
-std::vector<waypoint_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point_t_tC g, const double T, const double timeStep)
+std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point_t_tC g, const double T, const double timeStep)
{
int numSteps = computeNumSteps(T, timeStep);
static const double n = 3.; //degree
@@ -126,7 +125,7 @@ std::vector<waypoint_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point
Matrix3 p1X = skew(p1);
Matrix3 gX = skew( g);
double alpha = 1. / (T*T);
- std::vector<waypoint_t> wps;
+ std::vector<waypoint6_t> wps;
wps.push_back(w0(p0, p1, g, p0X, p1X, gX, alpha));
wps.push_back(w1(p0, p1, g, p0X, p1X, gX, alpha));
wps.push_back(w2(p0, p1, g, p0X, p1X, gX, alpha));
@@ -140,11 +139,11 @@ std::vector<waypoint_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point
return wps;
}
-std::vector<waypoint_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const double T, const double timeStep)
+std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const double T, const double timeStep)
{
int numSteps = computeNumSteps(T, timeStep);
double alpha = 1. / (T);
- std::vector<waypoint_t> wps;
+ std::vector<waypoint6_t> wps;
wps.push_back(u0(l0, alpha));
wps.push_back(u1(l0, alpha));
wps.push_back(u2(l0, alpha));
@@ -170,7 +169,7 @@ On the 6d curves, Ain x <= Aub
*/
std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData& cData, point_t_tC c0, point_t_tC dc0, point_t_tC l0, const bool useAngMomentum, const double T, const double timeStep, bool& fail)
{
- std::vector<waypoint_t> wps, wpL;
+ std::vector<waypoint6_t> wps, wpL;
wps = ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
if (useAngMomentum)
wpL = ComputeAllWaypointsAngularMomentum(l0, T, timeStep);
diff --git a/src/utils.cpp b/src/utils.cpp
index 0288ed357856bc3c7a4fb20a5a624f676cb911b5..b0ff218106a686cf126199d2a2147f8f14e00bf1 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -7,6 +7,19 @@
namespace bezier_com_traj
{
+waypoint_t initwp(const size_t rows, const size_t cols){
+ waypoint_t w;
+ w.first = MatrixXX::Zero(rows,cols);
+ w.second = VectorX::Zero(cols);
+}
+
+template<> waypoint9_t initwp<waypoint9_t>()
+{
+ waypoint9_t w;
+ w.first = Matrix39::Zero();
+ w.second = point3_t::Zero();
+ return w;
+}
template<> waypoint6_t initwp<waypoint6_t>()
{
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
index d8c4c212734577747142b7c26a98f92f7ca9f8e1..907f6a0f1dd9f27f038a48e918e1fb573c368bda 100644
--- a/src/waypoints_definition.cpp
+++ b/src/waypoints_definition.cpp
@@ -15,6 +15,7 @@
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh>
#include "boost/assign.hpp"
@@ -37,7 +38,8 @@ static const T_evalCurveAtTime evalCurveAtTimes = boost::assign::map_list_of
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::evaluateCurveAtTime)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)
(c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime)
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateCurveAtTime)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateCurveAtTime);
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
@@ -62,7 +64,8 @@ static const T_evalCurveAtTime evalCurveAtTimes = boost::assign::map_list_of
typedef std::map<ConstraintFlag,evalAccCurveAtTime > T_evalAccCurveAtTime;
typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
static const T_evalAccCurveAtTime EvalVelCurveAtTimes = boost::assign::map_list_of
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveAtTime);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateVelocityCurveAtTime)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateVelocityCurveAtTime);
/** @brief EvalVelCurveAtTimes compute the expression of the point on the curve dc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
@@ -91,7 +94,9 @@ static const T_evalAccCurveAtTime evalAccCurveAtTimes = boost::assign::map_list_
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)
(c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateAccelerationCurveAtTime);
+
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
@@ -115,7 +120,9 @@ static const T_evalAccCurveAtTime evalAccCurveAtTimes = boost::assign::map_list_
typedef std::map<ConstraintFlag,evalAccCurveAtTime > T_evalAccCurveAtTime;
typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
static const T_evalAccCurveAtTime evalJerkCurveAtTimes = boost::assign::map_list_of
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveAtTime);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::evaluateJerkCurveAtTime)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::evaluateJerkCurveAtTime);
+
/** @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
@@ -144,7 +151,9 @@ static const T_compConsWp compConsWps = boost::assign::map_list_of
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeConstantWaypoints)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)
(c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints)
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeConstantWaypoints)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeConstantWaypoints);
+
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states
* @param pData
@@ -164,18 +173,20 @@ static const T_compConsWp compConsWps = boost::assign::map_list_of
}
- typedef std::vector<waypoint3_t> (*compVelWp) (const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi);
+ typedef std::vector<waypoint_t> (*compVelWp) (const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag,compVelWp > T_compVelWp;
typedef T_compVelWp::const_iterator CIT_compVelWp;
static const T_compVelWp compVelWps = boost::assign::map_list_of
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeVelocityWaypoints)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeVelocityWaypoints);
+
/**
* @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<waypoint3_t> computeVelocityWaypoints(const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi)
+ std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi)
{
CIT_compVelWp cit = compVelWps.find(pData.constraints_.flag_);
if(cit != compVelWps.end())
@@ -188,18 +199,20 @@ static const T_compConsWp compConsWps = boost::assign::map_list_of
}
- typedef std::vector<waypoint3_t> (*compAccWp) (const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi);
+ typedef std::vector<waypoint_t> (*compAccWp) (const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag,compAccWp > T_compAccWp;
typedef T_compAccWp::const_iterator CIT_compAccWp;
static const T_compAccWp compAccWps = boost::assign::map_list_of
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeAccelerationWaypoints)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeAccelerationWaypoints);
+
/**
* @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<waypoint3_t> computeAccelerationWaypoints(const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi )
+ std::vector<waypoint_t> computeAccelerationWaypoints(const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi )
{
CIT_compAccWp cit = compAccWps.find(pData.constraints_.flag_);
if(cit != compAccWps.end())
@@ -212,18 +225,21 @@ static const T_compConsWp compConsWps = boost::assign::map_list_of
}
-typedef std::vector<waypoint3_t> (*compJerkWp) (const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi);
+typedef std::vector<waypoint_t> (*compJerkWp) (const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi);
typedef std::map<ConstraintFlag,compJerkWp > T_compJerkWp;
typedef T_compJerkWp::const_iterator CIT_compJerkWp;
static const T_compJerkWp compJerkWps = boost::assign::map_list_of
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeJerkWaypoints)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeJerkWaypoints);
+
+
/**
* @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<waypoint3_t> computeJerkWaypoints(const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi )
+std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData,double T,std::vector<bezier_t::point_t> pi )
{
CIT_compJerkWp cit = compJerkWps.find(pData.constraints_.flag_);
if(cit != compJerkWps.end())
@@ -246,7 +262,9 @@ static const T_compWp compWps = boost::assign::map_list_of
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeWwaypoints)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeWwaypoints)
(c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints)
- (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints);
+ (c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_j1_ddc1_dc1_c1::computeWwaypoints)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeWwaypoints);
+
/**
* @brief computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states
@@ -275,7 +293,9 @@ static const T_compFinalVelP compFinalVelPs = boost::assign::map_list_of
(c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeFinalVelocityPoint)
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeFinalVelocityPoint)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)
- (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint);
+ (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint)
+ (c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::flag , c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1::computeFinalVelocityPoint);
+
coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T)
{