From 65209187be3eb8efa1e23b54761bd0559dfbd7ba Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Fri, 25 May 2018 17:55:20 +0200
Subject: [PATCH] computeWwaypoint now return a vector of waypoint_t instead of
waypoint6_t
---
.../waypoints/waypoints_c0_dc0_c1.hh | 14 ++++---
.../waypoints/waypoints_c0_dc0_dc1.hh | 40 +++++--------------
.../waypoints/waypoints_c0_dc0_dc1_c1.hh | 18 +++++----
.../waypoints/waypoints_c0_dc0_ddc0_c1.hh | 18 +++++----
.../waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh | 22 +++++-----
.../waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh | 26 ++++++------
...waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh | 26 ++++++------
...points_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh | 6 ++-
...points_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh | 6 ++-
.../waypoints/waypoints_definition.hh | 2 +-
src/computeCOMTraj.cpp | 2 +-
src/waypoints_definition.cpp | 4 +-
12 files changed, 90 insertions(+), 94 deletions(-)
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
index cef126d..afe174a 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -53,8 +53,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -65,24 +67,24 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
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>();
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
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;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
index c4e2e28..d98b447 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
@@ -52,8 +52,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -65,54 +67,32 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
const double alpha = 1./(T2);
// equation of waypoints for curve w found with sympy
// TODO Apparently sympy equations are false ...
- /*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>(0,0) = 2.0*alpha*Matrix3::Identity();
- w1.first.block<3,3>(3,0) = 1.0*(-4.0*Cpi[0] + 6.0*Cpi[1])*alpha;
- w1.second.head<3>() = 1.0*(4.0*pi[0] - 6.0*pi[1])*alpha;
- w1.second.tail<3>() = 1.0*Cg*pi[1];
- wps.push_back(w1);
- waypoint6_t w2 = initwp<waypoint6_t>();
- w2.first.block<3,3>(0,0) = -2.0*alpha*Matrix3::Identity();
- w2.first.block<3,3>(3,0) = 1.0*(1.0*Cg* - 2.0*Cpi[0])*alpha;
- w2.second.head<3>() = 2.0*pi[0]*alpha;
- wps.push_back(w2);
- waypoint6_t w3 = initwp<waypoint6_t>();
- w3.first.block<3,3>(0,0) = -6*alpha*Matrix3::Identity();
- w3.first.block<3,3>(3,0) = 1.0*(1.0*Cg* - 6.0*Cpi[1])*alpha;
- w3.second.head<3>() = 6*pi[1]*alpha;
- wps.push_back(w3);
- return wps;*/
- waypoint6_t w0 = initwp<waypoint6_t>();
+
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>() = (-Cpi[0])*(12.0*pi[1]*alpha + g);
wps.push_back(w0);
- waypoint6_t w1 = initwp<waypoint6_t>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
w1.first.block<3,3>(0,0) = 3*alpha*Matrix3::Identity();
w1.first.block<3,3>(3,0) = skew(1.5 * (3*pi[1] - pi[0]))*alpha;
w1.second.head<3>() = 1.5 *alpha* (3* pi[0] - 5*pi[1]);
w1.second.tail<3>() = (3*alpha*pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3*pi[1] + pi[0]));
wps.push_back(w1);
- waypoint6_t w2 = initwp<waypoint6_t>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
w2.first.block<3,3>(0,0) = 0*alpha*Matrix3::Identity();
w2.first.block<3,3>(3,0) = skew(0.5*g - 3*alpha* pi[0] + 3*alpha*pi[1]);
w2.second.head<3>() = 3*alpha*(pi[0] - pi[1]);
w2.second.tail<3>() = 0.5 * Cg*pi[1];
wps.push_back(w2);
- waypoint6_t w3 = initwp<waypoint6_t>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
w3.first.block<3,3>(0,0) = -3*alpha*Matrix3::Identity();
w3.first.block<3,3>(3,0) = skew(g - 1.5 *alpha* (pi[1] + pi[0]));
w3.second.head<3>() = 1.5*alpha * (pi[1] + pi[0]);
wps.push_back(w3);
- waypoint6_t w4 = initwp<waypoint6_t>();
+ waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
w4.first.block<3,3>(0,0) = -6*alpha * Matrix3::Identity();
w4.first.block<3,3>(3,0) = skew(g - 6*alpha* pi[1]);
w4.second.head<3>() = 6*pi[1]*alpha;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
index c67f383..e7e8b7a 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -53,8 +53,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -65,37 +67,37 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
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>();
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w5 = initwp(DIM_POINT,DIM_VAR);
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;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
index 13b914e..59d2079 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -56,8 +56,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -69,34 +71,34 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
const double alpha = 1/(T2);
// equation of waypoints for curve w found with sympy
- waypoint6_t w0 = initwp<waypoint6_t>();
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w5 = initwp(DIM_POINT,DIM_VAR);
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;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
index b86bb91..217d1f5 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -60,8 +60,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -73,47 +75,47 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
const double alpha = 1/(T2);
// equation of waypoints for curve w found with sympy
- waypoint6_t w0 = initwp<waypoint6_t>();
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w5 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w6 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w7 = initwp(DIM_POINT,DIM_VAR);
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;
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
index 31c11e8..40a9461 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -63,8 +63,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -76,59 +78,59 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
const double alpha = 1/(T2);
// equation of waypoints for curve w found with sympy
- waypoint6_t w0 = initwp<waypoint6_t>();
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w5 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w6 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w7 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w8 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w9 = initwp(DIM_POINT,DIM_VAR);
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);
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 815f63d..a7d1f04 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
@@ -132,8 +132,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
return pi;
}
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 3;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
@@ -145,59 +147,59 @@ inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,doubl
const double alpha = 1/(T2);
// equation of waypoints for curve w found with sympy
- waypoint6_t w0 = initwp<waypoint6_t>();
+ waypoint_t w0 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w1 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w2 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w3 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w4 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w5 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w6 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w7 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w8 = initwp(DIM_POINT,DIM_VAR);
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>();
+ waypoint_t w9 = initwp(DIM_POINT,DIM_VAR);
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);
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
index 1c5202a..2850760 100644
--- 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
@@ -156,8 +156,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
}
//TODO
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 9;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
index e5a19e1..b8c3b5e 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
@@ -176,8 +176,10 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
}
//TODO
-inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
- std::vector<waypoint6_t> wps;
+inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T){
+ bezier_wp_t::t_point_t wps;
+ const int DIM_POINT = 6;
+ const int DIM_VAR = 15;
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
std::vector<Matrix3> Cpi;
for(std::size_t i = 0 ; i < pi.size() ; ++i){
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
index 724ac53..1ce7003 100644
--- a/include/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -100,7 +100,7 @@ waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData, const doubl
* @param T
* @return
*/
-std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T);
+bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T);
coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T);
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
index 35a2b0b..af3e369 100644
--- a/src/computeCOMTraj.cpp
+++ b/src/computeCOMTraj.cpp
@@ -26,7 +26,7 @@ typedef std::pair<double,point3_t> coefs_t;
std::vector<waypoint6_t> computeDiscretizedWwaypoints(const ProblemData& pData,double T, const T_time& timeArray)
{
- std::vector<waypoint6_t> wps = computeWwaypoints(pData,T);
+ bezier_wp_t::t_point_t wps = computeWwaypoints(pData,T);
std::vector<waypoint6_t> res;
std::vector<spline::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size()-1);
double t, b;
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
index e9ccd26..83272ef 100644
--- a/src/waypoints_definition.cpp
+++ b/src/waypoints_definition.cpp
@@ -313,7 +313,7 @@ std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData,double T,s
}
-typedef std::vector<waypoint6_t> (*compWp) (const ProblemData& pData,double T);
+typedef bezier_wp_t::t_point_t (*compWp) (const ProblemData& pData,double T);
typedef std::map<ConstraintFlag,compWp > T_compWp;
typedef T_compWp::const_iterator CIT_compWp;
static const T_compWp compWps = boost::assign::map_list_of
@@ -332,7 +332,7 @@ static const T_compWp compWps = boost::assign::map_list_of
* @param T
* @return
*/
- std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T)
+ bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,double T)
{
CIT_compWp cit = compWps.find(pData.constraints_.flag_);
if(cit != compWps.end())
--
GitLab