piecewise has a template curve param

include/curves/bezier_curve.h

@@ -40,7 +40,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   typedef typename t_point_t::const_iterator cit_point_t;
   typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
   typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
-  typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
+  typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_curve_t> piecewise_curve_t;
   typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t;  // parent class
   typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
 

include/curves/fwd.h

@@ -29,7 +29,7 @@ template <typename Time, typename Numeric, bool Safe, typename Point,
   struct exact_cubic;
 
 template <typename Time, typename Numeric, bool Safe, typename Point,
-            typename Point_derivate>
+            typename Point_derivate, typename CurveType>
   struct piecewise_curve;
 
 template <typename Time, typename Numeric, bool Safe,typename Point, typename T_Point>
@@ -82,19 +82,19 @@ typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
 typedef exact_cubic<double, double, true, pointX_t,t_pointX_t, polynomial_t> exact_cubic_t;
 typedef bezier_curve<double, double, true, pointX_t> bezier_t;
 typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
-typedef piecewise_curve <double, double, true, pointX_t,pointX_t> piecewise_t;
+typedef piecewise_curve <double, double, true, pointX_t,pointX_t, curve_abc_t> piecewise_t;
 
 // definition of all curves class with point3 as return type:
 typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
 typedef exact_cubic<double, double, true, point3_t,t_point3_t, polynomial_t> exact_cubic3_t;
 typedef bezier_curve<double, double, true, point3_t> bezier3_t;
 typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
-typedef piecewise_curve <double, double, true, point3_t,point3_t> piecewise3_t;
+typedef piecewise_curve <double, double, true, point3_t,point3_t, curve_3_t> piecewise3_t;
 
 // special curves with return type fixed:
 typedef SO3Linear<double, double, true> SO3Linear_t;
 typedef SE3Curve<double, double, true> SE3Curve_t;
-typedef piecewise_curve <double, double, true,  transform_t, point6_t> piecewise_SE3_t;
+typedef piecewise_curve <double, double, true,  transform_t, point6_t, curve_SE3_t> piecewise_SE3_t;
 
 }
 

include/curves/piecewise_curve.h

@@ -25,7 +25,7 @@ namespace curves {
 ///
 template <typename Time = double, typename Numeric = Time, bool Safe = false,
           typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
-          typename Point_derivate = Point >
+          typename Point_derivate = Point, typename CurveType = curve_abc<Time, Numeric, Safe, Point,Point_derivate> >
 struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
   typedef Point point_t;
   typedef Point_derivate   point_derivate_t;
@@ -33,11 +33,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
   typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> > t_point_derivate_t;
   typedef Time time_t;
   typedef Numeric num_t;
-  typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
+  typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> base_curve_t; // parent class
+  typedef CurveType curve_t; // contained curves base class
   typedef boost::shared_ptr<curve_t> curve_ptr_t;
   typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
   typedef typename std::vector<Time> t_time_t;
-  typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate> piecewise_curve_t;
+  typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate,CurveType> piecewise_curve_t;
  public:
   /// \brief Empty constructor. Add at least one curve to call other class functions.
   ///
@@ -97,7 +98,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
     return true;
   }
 
-  virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+  virtual bool isApprox(const base_curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
     const piecewise_curve_t* other_cast = dynamic_cast<const piecewise_curve_t*>(other);
     if(other_cast)
       return isApprox(*other_cast,prec);
@@ -435,7 +436,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
     if (version) {
       // Do something depending on version ?
     }
-    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_t);
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
     ar& boost::serialization::make_nvp("dim", dim_);
     ar& boost::serialization::make_nvp("curves", curves_);
     ar& boost::serialization::make_nvp("time_curves", time_curves_);
@@ -446,8 +447,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
 };  // End struct piecewise curve
 
 
-template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate>
-const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate >::MARGIN(0.001);
+template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate, typename CurveType>
+const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate, CurveType >::MARGIN(0.001);
 
 }  // namespace curves
 

python/curves_python.cpp

@@ -176,6 +176,21 @@ piecewise_SE3_t* wrapPiecewiseSE3EmptyConstructor() {
   return new piecewise_SE3_t();
 }
 
+typedef bezier_t::piecewise_curve_t piecewise_bezier_t;
+piecewise_bezier_t* wrapPiecewiseBezierConstructor(const bezier_t::bezier_curve_ptr_t& curve) {
+  return new piecewise_bezier_t(curve);
+}
+piecewise_bezier_t* wrapPiecewiseBezierEmptyConstructor() {
+  return new piecewise_bezier_t();
+}
+typedef bezier_linear_variable_t::piecewise_curve_t piecewise_linear_bezier_t;
+piecewise_linear_bezier_t* wrapPiecewiseBezierLinearConstructor(const bezier_linear_variable_t::bezier_curve_ptr_t& curve) {
+  return new piecewise_linear_bezier_t(curve);
+}
+piecewise_linear_bezier_t* wrapPiecewiseBezierLinearEmptyConstructor() {
+  return new piecewise_linear_bezier_t();
+}
+
 static piecewise_t discretPointToPolynomialC0(const pointX_list_t& points,
                                                                const time_waypoints_t& time_points) {
   t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
@@ -708,6 +723,68 @@ BOOST_PYTHON_MODULE(curves) {
       .def(bp::self != bp::self)
       ;
 
+  class_<piecewise_bezier_t, bases<curve_abc_t> >("piecewise_bezier", init<>())
+      .def("__init__",
+           make_constructor(&wrapPiecewiseBezierConstructor, default_call_policies(), arg("curve")),
+           "Create a peicewise Bezier curve containing the given curve.")
+          .def("__init__", make_constructor(&wrapPiecewiseBezierEmptyConstructor),
+          "Create an empty piecewise-Beziercurve.")
+      .def("append", &piecewise_bezier_t::add_curve_ptr,
+           "Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
+           "where T_{min} is equal toT_{max} of the actual piecewise curve.")
+      .def("is_continuous", &piecewise_bezier_t::is_continuous,
+           "Check if the curve is continuous at the given order.",args("self,order"))
+      .def("curve_at_index", &piecewise_bezier_t::curve_at_index,
+           return_value_policy<copy_const_reference>())
+      .def("curve_at_time", &piecewise_bezier_t::curve_at_time, return_value_policy<copy_const_reference>())
+      .def("num_curves", &piecewise_bezier_t::num_curves)
+      .def("saveAsText", &piecewise_bezier_t::saveAsText<piecewise_bezier_t>, bp::args("filename"),
+           "Saves *this inside a text file.")
+      .def("loadFromText", &piecewise_bezier_t::loadFromText<piecewise_bezier_t>,
+           bp::args("filename"), "Loads *this from a text file.")
+      .def("saveAsXML", &piecewise_bezier_t::saveAsXML<piecewise_bezier_t>,
+           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+      .def("loadFromXML", &piecewise_bezier_t::loadFromXML<piecewise_bezier_t>,
+           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+      .def("saveAsBinary", &piecewise_bezier_t::saveAsBinary<piecewise_bezier_t>,
+           bp::args("filename"), "Saves *this inside a binary file.")
+      .def("loadFromBinary", &piecewise_bezier_t::loadFromBinary<piecewise_bezier_t>,
+           bp::args("filename"), "Loads *this from a binary file.")
+      .def(bp::self == bp::self)
+      .def(bp::self != bp::self)
+      ;
+
+  class_<piecewise_linear_bezier_t, bases<curve_abc_t> >("piecewise_bezier_linear", init<>())
+      .def("__init__",
+           make_constructor(&wrapPiecewiseBezierLinearConstructor, default_call_policies(), arg("curve")),
+           "Create a peicewise Bezier curve containing the given curve.")
+          .def("__init__", make_constructor(&wrapPiecewiseBezierLinearEmptyConstructor),
+          "Create an empty piecewise-Beziercurve.")
+      .def("append", &piecewise_linear_bezier_t::add_curve_ptr,
+           "Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
+           "where T_{min} is equal toT_{max} of the actual piecewise curve.")
+      .def("is_continuous", &piecewise_linear_bezier_t::is_continuous,
+           "Check if the curve is continuous at the given order.",args("self,order"))
+      .def("curve_at_index", &piecewise_linear_bezier_t::curve_at_index,
+           return_value_policy<copy_const_reference>())
+      .def("curve_at_time", &piecewise_linear_bezier_t::curve_at_time, return_value_policy<copy_const_reference>())
+      .def("num_curves", &piecewise_linear_bezier_t::num_curves)
+      .def("saveAsText", &piecewise_linear_bezier_t::saveAsText<piecewise_linear_bezier_t>, bp::args("filename"),
+           "Saves *this inside a text file.")
+      .def("loadFromText", &piecewise_linear_bezier_t::loadFromText<piecewise_linear_bezier_t>,
+           bp::args("filename"), "Loads *this from a text file.")
+      .def("saveAsXML", &piecewise_linear_bezier_t::saveAsXML<piecewise_linear_bezier_t>,
+           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+      .def("loadFromXML", &piecewise_linear_bezier_t::loadFromXML<piecewise_linear_bezier_t>,
+           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+      .def("saveAsBinary", &piecewise_linear_bezier_t::saveAsBinary<piecewise_linear_bezier_t>,
+           bp::args("filename"), "Saves *this inside a binary file.")
+      .def("loadFromBinary", &piecewise_linear_bezier_t::loadFromBinary<piecewise_linear_bezier_t>,
+           bp::args("filename"), "Loads *this from a binary file.")
+      .def(bp::self == bp::self)
+      .def(bp::self != bp::self)
+      ;
+
   class_<piecewise_SE3_t, bases<curve_SE3_t> >("piecewise_SE3", init<>())
       .def("__init__", make_constructor(&wrapPiecewiseSE3Constructor, default_call_policies(), arg("curve")),
       "Create a piecewise-se3 curve containing the given se3 curve.")