diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index e146c9c9fddeb625e9ff533ef6d81dfa23d37a32..d2f02a33e0f08d98eba9b09c2a88a232aa5e0483 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -333,9 +333,9 @@ namespace curves
}
t_point_t wps_first(size_),wps_second(size_);
const Numeric u = (t-T_min_)/(T_max_-T_min_);
- wps_first[0] = control_points_.front();
- wps_second[degree_] = control_points_.back();
t_point_t casteljau_pts = waypoints();
+ wps_first[0] = casteljau_pts.front();
+ wps_second[degree_] = casteljau_pts.back();
size_t id = 1;
while(casteljau_pts.size() > 1)
{
@@ -344,31 +344,37 @@ namespace curves
wps_second[degree_-id] = casteljau_pts.back();
++id;
}
-
bezier_curve_t c_first(wps_first.begin(), wps_first.end(),T_min_,t,mult_T_);
bezier_curve_t c_second(wps_second.begin(), wps_second.end(),t, T_max_,mult_T_);
return std::make_pair(c_first,c_second);
}
- bezier_curve_t extract(const Numeric t1, const Numeric t2){
+ /// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve
+ /// defined between \f$[T_{min},T_{max}]\f$ with \f$T_{min} \leq t_1 \leq t_2 \leq T_{max}\f$.
+ /// \param t1 : start time of bezier curve extracted.
+ /// \param t2 : end time of bezier curve extracted.
+ /// \return bezier curve extract defined between \f$[t_1,t_2]\f$.
+ ///
+ bezier_curve_t extract(const Numeric t1, const Numeric t2)
+ {
if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
{
throw std::out_of_range("In Extract curve : times out of bounds");
}
- if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN)
+ if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN) // t1=T_min and t2=T_max
{
return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
}
- if (fabs(t1-T_min_)<MARGIN)
+ if (fabs(t1-T_min_)<MARGIN) // t1=T_min
{
return split(t2).first;
}
- if (fabs(t2-T_max_)<MARGIN)
+ if (fabs(t2-T_max_)<MARGIN) // t2=T_max
{
return split(t1).second;
}
std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
- return c_split.second.split(t2-t1).first;
+ return c_split.second.split(t2).first;
}
private:
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 27e23728943eb2f2f274280c90197cfd977daa40..3e922505dc0e1a37a52e867d7ef338dc015f97d7 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -44,12 +44,16 @@ typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, bezier_
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::exact_cubic <real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
+// Bezier 3
+typedef curves::bezier_curve <real, real, true, Eigen::Vector3d> bezier3_t;
+
typedef curves::Bern<double> bernstein_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial_curve_t)
@@ -244,6 +248,30 @@ namespace curves
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
+ /** BEGIN bezier3 curve**/
+ class_<bezier3_t>("bezier3", init<>())
+ .def("__init__", make_constructor(&wrapBezierConstructor))
+ .def("__init__", make_constructor(&wrapBezierConstructorBounds))
+ .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
+ .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
+ .def("min", &bezier3_t::min)
+ .def("max", &bezier3_t::max)
+ .def("dim", &bezier3_t::dim)
+ .def("__call__", &bezier3_t::operator())
+ .def("derivate", &bezier3_t::derivate)
+ .def("compute_derivate", &bezier3_t::compute_derivate)
+ .def("compute_primitive", &bezier3_t::compute_primitive)
+ .def("saveAsText", &bezier3_t::saveAsText<bezier3_t>,bp::args("filename"),"Saves *this inside a text file.")
+ .def("loadFromText",&bezier3_t::loadFromText<bezier3_t>,bp::args("filename"),"Loads *this from a text file.")
+ .def("saveAsXML",&bezier3_t::saveAsXML<bezier3_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
+ .def("loadFromXML",&bezier3_t::loadFromXML<bezier3_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
+ .def("saveAsBinary",&bezier3_t::saveAsBinary<bezier3_t>,bp::args("filename"),"Saves *this inside a binary file.")
+ .def("loadFromBinary",&bezier3_t::loadFromBinary<bezier3_t>,bp::args("filename"),"Loads *this from a binary file.")
+ .def_readonly("degree", &bezier3_t::degree_)
+ .def_readonly("nbWaypoints", &bezier3_t::size_)
+ //.def(SerializableVisitor<bezier_t>())
+ ;
+ /** END bezier3 curve**/
/** BEGIN bezier curve**/
class_<bezier_t>("bezier", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor))
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 8c6c70b9ff41cd2ac81305ce63f0ab5083e823b8..5f44088a7d1faa0aa5d4afaefedfd2f044780a88 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -212,10 +212,10 @@ void BezierCurveTest(bool& error)
std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
for(double d = 1.; d <2.; d+=0.1)
{
- ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
- ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
- ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
- ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
+ ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
+ ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
+ ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
+ ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
}
bool error_in(true);
try
@@ -920,62 +920,82 @@ void BezierSplitCurve(bool& error)
double t_min = 0.2;
double t_max = 10;
double aux0, aux1;
+ std::string errMsg0("BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
+ std::string errMsg1("BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original");
+ std::string errMsg2("BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original");
+ std::string errMsg3("BezierSplitCurve, ERROR while checking value on curve and curves splitted");
+ std::string errMsg4("BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve");
+ std::string errMsg5("BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original");
+ std::string errMsg6("BezierSplitCurve, ERROR while checking value on curve extracted");
for(size_t i = 0 ; i < 1 ; ++i)
{
- // build a random curve and split it at random time :
- //std::cout<<"build a random curve"<<std::endl;
- point_t a;
- std::vector<point_t> wps;
- for(size_t j = 0 ; j <= n ; ++j)
- {
- wps.push_back(randomPoint(-10.,10.));
- }
- double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
- double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
- double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
-
- bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
- std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
-
- // test on splitted curves :
- if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
- {
- error = true;
- std::cout<<"BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve"<<std::endl;
- }
- aux0 = c.max()-c.min();
- aux1 = (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min());
- if(!QuasiEqual(aux0, aux1))
- {
- error = true;
- std::cout<<"BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original"<<std::endl;
- }
- if(!QuasiEqual(cs.first.max(), ts))
- {
- error = true;
- std::cout<<"BezierSplitCurve, ERROR timing of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- std::string errmsg("BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
- ComparePoints(c(t0), cs.first(t0), errmsg, error);
- errmsg = "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original";
- ComparePoints(cs.first(ts), cs.second(ts), errmsg, error);
- errmsg = "BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original";
- ComparePoints(c(t1), cs.second(cs.second.max()), errmsg, error);
-
- // check along curve :
- double ti = t0;
- errmsg = "BezierSplitCurve, ERROR while checking value on curve and curves splitted";
- while(ti <= ts)
- {
- ComparePoints(cs.first(ti), c(ti), errmsg, error);
- ti += 0.01;
- }
- while(ti <= t1)
- {
- ComparePoints(cs.second(ti), c(ti), errmsg, error);
- ti += 0.01;
- }
+ // build a random curve and split it at random time :
+ //std::cout<<"build a random curve"<<std::endl;
+ point_t a;
+ std::vector<point_t> wps;
+ for(size_t j = 0 ; j <= n ; ++j)
+ {
+ wps.push_back(randomPoint(-10.,10.));
+ }
+ double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
+ double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
+ double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
+ bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
+ std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
+ // test on splitted curves :
+ if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
+ {
+ error = true;
+ std::cout<<errMsg4<<std::endl;
+ }
+ aux0 = c.max()-c.min();
+ aux1 = (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min());
+ if(!QuasiEqual(aux0, aux1))
+ {
+ error = true;
+ std::cout<<errMsg5<<std::endl;
+ }
+ if(!QuasiEqual(cs.first.max(), ts))
+ {
+ error = true;
+ std::cout<<errMsg0<<std::endl;
+ }
+ ComparePoints(c(t0), cs.first(t0), errMsg0, error);
+ ComparePoints(cs.first(ts), cs.second(ts), errMsg1, error);
+ ComparePoints(c(t1), cs.second(cs.second.max()), errMsg2, error);
+ // check along curve :
+ double ti = t0;
+ while(ti <= ts)
+ {
+ ComparePoints(cs.first(ti), c(ti), errMsg3, error);
+ ti += 0.01;
+ }
+ while(ti <= t1)
+ {
+ ComparePoints(cs.second(ti), c(ti), errMsg3, error);
+ ti += 0.01;
+ }
+ // Test extract function
+ bezier_curve_t bezier_extracted0 = c.extract(t0+0.01,t1-0.01); // T_min < t0 < t1 < T_max
+ for(double t=bezier_extracted0.min(); t<bezier_extracted0.max(); t+=0.01)
+ {
+ ComparePoints(bezier_extracted0(t),c(t),errMsg6, error);
+ }
+ bezier_curve_t bezier_extracted1 = c.extract(t0,t1-0.01); // T_min = t0 < t1 < T_max
+ for(double t=bezier_extracted1.min(); t<bezier_extracted1.max(); t+=0.01)
+ {
+ ComparePoints(bezier_extracted1(t),c(t),errMsg6, error);
+ }
+ bezier_curve_t bezier_extracted2 = c.extract(t0+0.01,t1); // T_min < t0 < t1 = T_max
+ for(double t=bezier_extracted2.min(); t<bezier_extracted2.max(); t+=0.01)
+ {
+ ComparePoints(bezier_extracted2(t),c(t),errMsg6, error);
+ }
+ bezier_curve_t bezier_extracted3 = c.extract(t0,t1); // T_min = t0 < t1 = T_max
+ for(double t=bezier_extracted3.min(); t<bezier_extracted3.max(); t+=0.01)
+ {
+ ComparePoints(bezier_extracted3(t),c(t),errMsg6, error);
+ }
}
}
@@ -983,88 +1003,103 @@ void BezierSplitCurve(bool& error)
/* cubic hermite spline function test */
void CubicHermitePairsPositionDerivativeTest(bool& error)
{
- std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
- std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
- std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
- std::vector< pair_point_tangent_t > control_points;
- point_t res1;
- point_t p0(0.,0.,0.);
- point_t p1(1.,2.,3.);
- point_t p2(4.,4.,4.);
- point_t t0(0.5,0.5,0.5);
- point_t t1(0.1,0.2,-0.5);
- point_t t2(0.1,0.2,0.3);
- std::vector< double > time_control_points, time_control_points_test;
- // Two pairs
- control_points.clear();
- control_points.push_back(pair_point_tangent_t(p0,t0));
- control_points.push_back(pair_point_tangent_t(p1,t1));
- time_control_points.push_back(0.); // Time at P0
- time_control_points.push_back(1.); // Time at P1
- // Create cubic hermite spline
- cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
- // Dimension
- if (cubic_hermite_spline_1Pair.dim() != 3)
- {
- error = true;
- std::cout << "Cubic hermite spline test, Error : Dimension of curve is wrong\n";
- }
- //Check
- res1 = cubic_hermite_spline_1Pair(0.); // t=0
- ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_1Pair(1.); // t=1
- ComparePoints(p1, res1, errmsg1, error);
- // Test derivative : two pairs
- res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
- ComparePoints(t0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
- ComparePoints(t1, res1, errmsg3, error);
- // Three pairs
- control_points.push_back(pair_point_tangent_t(p2,t2));
- time_control_points.clear();
- time_control_points.push_back(0.); // Time at P0
- time_control_points.push_back(2.); // Time at P1
- time_control_points.push_back(5.); // Time at P2
- cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
- //Check
- res1 = cubic_hermite_spline_2Pairs(0.); // t=0
- ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_2Pairs(2.); // t=2
- ComparePoints(p1, res1, errmsg2, error);
- res1 = cubic_hermite_spline_2Pairs(5.); // t=5
- ComparePoints(p2, res1, errmsg1, error);
- // Test derivative : three pairs
- res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
- ComparePoints(t0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
- ComparePoints(t1, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
- ComparePoints(t2, res1, errmsg3, error);
- // Test time control points by default [0,1] => with N control points :
- // Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
- time_control_points_test.clear();
- time_control_points_test.push_back(0.); // Time at P0
- time_control_points_test.push_back(0.5); // Time at P1
- time_control_points_test.push_back(1.0); // Time at P2
- cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
- res1 = cubic_hermite_spline_2Pairs(0.); // t=0
- ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
- ComparePoints(p1, res1, errmsg2, error);
- res1 = cubic_hermite_spline_2Pairs(1.); // t=1
- ComparePoints(p2, res1, errmsg1, error);
- // Test getTime
try
{
+ std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
+ std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
+ std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
+ std::vector< pair_point_tangent_t > control_points;
+ point_t res1;
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
+ point_t t0(0.5,0.5,0.5);
+ point_t t1(0.1,0.2,-0.5);
+ point_t t2(0.1,0.2,0.3);
+ std::vector< double > time_control_points, time_control_points_test;
+ // Two pairs
+ control_points.clear();
+ control_points.push_back(pair_point_tangent_t(p0,t0));
+ control_points.push_back(pair_point_tangent_t(p1,t1));
+ time_control_points.push_back(0.); // Time at P0
+ time_control_points.push_back(1.); // Time at P1
+ // Create cubic hermite spline
+ cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
+ // Dimension
+ if (cubic_hermite_spline_1Pair.dim() != 3)
+ {
+ error = true;
+ std::cout << "Cubic hermite spline test, Error : Dimension of curve is wrong\n";
+ }
+ //Check
+ res1 = cubic_hermite_spline_1Pair(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_1Pair(1.); // t=1
+ ComparePoints(p1, res1, errmsg1, error);
+ // Test derivative : two pairs
+ res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ // Three pairs
+ control_points.push_back(pair_point_tangent_t(p2,t2));
+ time_control_points.clear();
+ time_control_points.push_back(0.); // Time at P0
+ time_control_points.push_back(2.); // Time at P1
+ time_control_points.push_back(5.); // Time at P2
+ cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
+ //Check
+ res1 = cubic_hermite_spline_2Pairs(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_2Pairs(2.); // t=2
+ ComparePoints(p1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(5.); // t=5
+ ComparePoints(p2, res1, errmsg1, error);
+ // Test derivative : three pairs
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
+ ComparePoints(t2, res1, errmsg3, error);
+ // Test time control points by default [0,1] => with N control points :
+ // Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
+ time_control_points_test.clear();
+ time_control_points_test.push_back(0.); // Time at P0
+ time_control_points_test.push_back(0.5); // Time at P1
+ time_control_points_test.push_back(1.0); // Time at P2
+ cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
+ res1 = cubic_hermite_spline_2Pairs(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
+ ComparePoints(p1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(1.); // t=1
+ ComparePoints(p2, res1, errmsg1, error);
+ // Test getTime
+ try
+ {
cubic_hermite_spline_2Pairs.getTime();
- }
- catch(...)
- {
+ }
+ catch(...)
+ {
error = false;
+ }
+ if(error)
+ {
+ std::cout << "Cubic hermite spline test, Error when calling getTime\n";
+ }
+ // Test derivative : three pairs, time default
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
+ ComparePoints(t2, res1, errmsg3, error);
}
- if(error)
+ catch(...)
{
- std::cout << "Cubic hermite spline test, Error when calling getTime\n";
+ error = true;
+ std::cout<<"Error in CubicHermitePairsPositionDerivativeTest"<<std::endl;
}
}