Merge tag 'v0.3.0'

Release of version 0.3.0.

.gitignore

@@ -6,3 +6,4 @@ build-rel/
 .*~
 *.user
 *.pyc
+*.ipynb_checkpoints

CMakeLists.txt

-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
-
-INCLUDE(cmake/base.cmake)
-INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/test.cmake)
-INCLUDE(cmake/python.cmake)
-INCLUDE(cmake/hpp.cmake)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 
 SET(PROJECT_ORG loco-3d)
 SET(PROJECT_NAME curves)
 SET(PROJECT_DESCRIPTION
   "template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics."
   )
+SET(PROJECT_URL "http://gepgitlab.laas.fr/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/test.cmake)
+INCLUDE(cmake/python.cmake)
 
 # Disable -Werror on Unix for now.
 SET(CXX_DISABLE_WERROR True)
 
-SETUP_HPP_PROJECT()
+COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
+PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
 ADD_REQUIRED_DEPENDENCY(eigen3)
+ADD_OPTIONAL_DEPENDENCY(pinocchio)
+OPTION(CURVES_WITH_PINOCCHIO_SUPPORT "Build with pinocchio support" ${PINOCCHIO_FOUND})
+IF(CURVES_WITH_PINOCCHIO_SUPPORT)
+  IF(NOT PINOCCHIO_FOUND)
+    MESSAGE(FATAL_ERROR "CURVES_WITH_PINOCCHIO_SUPPORT selected, but pinocchio has not been found")
+  ENDIF(NOT PINOCCHIO_FOUND)
+  ADD_DEFINITIONS(-DCURVES_WITH_PINOCCHIO_SUPPORT)
+  PKG_CONFIG_APPEND_CFLAGS("-DCURVES_WITH_PINOCCHIO_SUPPORT")
+ENDIF(CURVES_WITH_PINOCCHIO_SUPPORT)
 
 SET(BOOST_COMPONENTS unit_test_framework serialization)
 
@@ -28,7 +38,7 @@ IF(BUILD_PYTHON_INTERFACE)
 
   STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
 
-  ADD_REQUIRED_DEPENDENCY("eigenpy")
+  ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.6.8")
   SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
 ENDIF(BUILD_PYTHON_INTERFACE)
 
@@ -44,5 +54,3 @@ ENDIF(BUILD_PYTHON_INTERFACE)
 
 ADD_SUBDIRECTORY(include/curves)
 ADD_SUBDIRECTORY(tests)
-
-SETUP_HPP_PROJECT_FINALIZE()

README.md

@@ -12,6 +12,8 @@ To do so, tools are provided to:
  - constrain initial / end velocities and acceleration for the spline.
  - constrain take-off and landing phases to follow a straight line along a given normal (to avoid undesired collisions between the effector and the contact surface)
  - automatically handle 3d rotation of the effector.
+ - create curves in SO3
+ - support partial symbolic differentiation of curves. You can represent control points as linear variables, and integrate / differentiate those variable curves. You can also compute the cross product of two curves, which is relevant for centroidal dynamics.
 
 Several type of formulation are provided:
  - Polynomials
@@ -47,20 +49,8 @@ exact_cubic_t* eff_traj = effector_spline(waypoints.begin(),waypoints.end());
 ```
 If rotation of the effector must be considered, the code is almost the same:
 
-```
-// initial rotation is 0, end rotation is a rotation by Pi around x axis
-quat_t init_rot(0,0,0,1), end_rot(1,0,0,0);
-effector_spline_rotation eff_traj_rot(waypoints.begin(),waypoints.end(), init_quat, end_quat);
+The library is template-based, thus generic:  the curves can be of any dimension, and can be implemented in double, float  ...
 
-// evaluate spline
-eff_traj_rot(0.); // (0,0,0,0,0,0,1)
-eff_traj_rot(1.); // (0.5,0.5,0.5,0.707107,0,0,0.707107) // Pi/2 around x axis
-eff_traj_rot(2.); // (0,0,0,1,0,0,0)
-```
-
-Additional parameters for the same methods an be used to specify parameters for the take off and
-landing phases: height and duration of the phase, and along which normal.
-Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library
 
 Installation
 -------------
@@ -77,34 +67,38 @@ This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots
 To handle this with cmake, use the recursive option to clone the repository.
 For instance, using http:
 ```
-git clone --recursive https://github.com/loco-3d/curves.git
+git clone --recursive https://github.com/loco-3d/curves $CURVES_DIR
 ```
+
 The library is header only, so the build only serves to build the tests and python bindings:
 
-```
-	cd curves && mkdir build && cd build
-	cmake .. && make
-	make test
+```sh
+cd $CURVES_DIR && mkdir build && cd build
+cmake .. && make && make test
 ```
 
 If everything went fine you should obtain the following output:
-```
-performing tests...
-no errors found
+```sh
+100% tests passed, 0 tests failed out of 3
 ```
 ### Optional: Python bindings installation
-To install the Python bindings first enable the BUILD_PYTHON_INTERFACE option:
+To install the Python bindings first enable the `BUILD_PYTHON_INTERFACE` option:
 ```
 cmake -DBUILD_PYTHON_INTERFACE=ON ..
 ```
 
 Then rebuild the library:
 ```
-cd curves/build
-make install
+cd ${CURVES_DIR}/build
+make && make test
 ```
 To see example of use, you can refer to the [test file](https://github.com/loco-3d/curves/blob/master/python/test/test.py)
 which is rather self explanatory:
 
-In spite of an exhaustive documentation, please refer to the C++ documentation, which mostly applies
-to python.
+In spite of an exhaustive documentation, please refer to the C++ documentation, which mostly applies to python.
+
+Documentation and tutorial
+-------------
+
+For a python tutorial, you can refer to the [jupyter notebook](https://github.com/loco-3d/curves/blob/devel/python/test/sandbox/test.ipynb).
+The [test file](https://github.com/loco-3d/curves/blob/master/python/test/test.py) is more exhaustive and rather self explanatory.

cmake @ e7892518

-Subproject commit bbdade93ad2a4b7818cb5d5b9f0bf131627e0b71
+Subproject commit e7892518eb62e1d20823307af48d6041f78a8da4

include/curves/CMakeLists.txt

@@ -10,14 +10,19 @@ SET(${PROJECT_NAME}_HEADERS
   curve_constraint.h
   quintic_spline.h
   linear_variable.h
+  quadratic_variable.h
   cubic_hermite_spline.h
   piecewise_curve.h
+  so3_linear.h
+  se3_curve.h
+  fwd.h
   )
 
 INSTALL(FILES
   ${${PROJECT_NAME}_HEADERS}
-  DESTINATION include/curves
+  DESTINATION include/${PROJECT_NAME}
   )
 
 ADD_SUBDIRECTORY(helpers)
+ADD_SUBDIRECTORY(optimization)
 ADD_SUBDIRECTORY(serialization)

include/curves/bernstein.h

@@ -45,6 +45,13 @@ struct Bern {
     return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
   }
 
+  virtual bool operator==(const Bern& other) const {
+    return curves::isApprox<Numeric>(m_minus_i, other.m_minus_i) && curves::isApprox<Numeric>(i_, other.i_) &&
+           curves::isApprox<Numeric>(bin_m_i_, other.bin_m_i_);
+  }
+
+  virtual bool operator!=(const Bern& other) const { return !(*this == other); }
+
   /* Attributes */
   Numeric m_minus_i;
   Numeric i_;

include/curves/bezier_curve.h

@@ -12,6 +12,7 @@
 #include "curve_abc.h"
 #include "bernstein.h"
 #include "curve_constraint.h"
+#include "piecewise_curve.h"
 
 #include "MathDefs.h"
 
@@ -30,12 +31,18 @@ template <typename Time = double, typename Numeric = Time, bool Safe = false,
           typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
 struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   typedef Point point_t;
+  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
+  typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
   typedef Time time_t;
   typedef Numeric num_t;
   typedef curve_constraints<point_t> curve_constraints_t;
   typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
   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, 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;
 
   /* Constructors - destructors */
  public:
@@ -133,6 +140,38 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     }
   }
 
+  /**
+   * @brief isApprox check if other and *this are approximately equals.
+   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see
+   * isEquivalent
+   * @param other the other curve to check
+   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
+   * @return true is the two curves are approximately equals
+   */
+  bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
+    bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
+                 dim_ == other.dim() && degree_ == other.degree() && size_ == other.size_ &&
+                 curves::isApprox<Numeric>(mult_T_, other.mult_T_) && bernstein_ == other.bernstein_;
+    if (!equal) return false;
+    for (size_t i = 0; i < size_; ++i) {
+      if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec)) return false;
+    }
+    return true;
+  }
+
+  virtual bool isApprox(const curve_abc_t* other,
+                        const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
+    const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(other);
+    if (other_cast)
+      return isApprox(*other_cast, prec);
+    else
+      return false;
+  }
+
+  virtual bool operator==(const bezier_curve_t& other) const { return isApprox(other); }
+
+  virtual bool operator!=(const bezier_curve_t& other) const { return !(*this == other); }
+
   ///  \brief Compute the derived curve at order N.
   ///  Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
   ///  \param order : order of derivative.
@@ -153,6 +192,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     return deriv.compute_derivate(order - 1);
   }
 
+  ///  \brief Compute the derived curve at order N.
+  ///  \param order : order of derivative.
+  ///  \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+  bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
+    return new bezier_curve_t(compute_derivate(order));
+  }
+
   ///  \brief Compute the primitive of the curve at order N.
   ///  Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
   ///  At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
@@ -184,10 +230,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   ///  \param t : time when to evaluate the curve.
   ///  \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
   ///
-  virtual point_t derivate(const time_t t, const std::size_t order) const {
-    bezier_curve_t deriv = compute_derivate(order);
-    return deriv(t);
-  }
+  virtual point_t derivate(const time_t t, const std::size_t order) const { return compute_derivate(order)(t); }
 
   /// \brief Evaluate all Bernstein polynomes for a certain degree.
   /// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
@@ -241,7 +284,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
 
   const point_t waypointAtIndex(const std::size_t index) const {
     point_t waypoint;
-    if (index <= control_points_.size()) {
+    if (index < control_points_.size()) {
       waypoint = control_points_[index];
     }
     return waypoint;
@@ -297,7 +340,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   /// \param u : unNormalized time.
   /// \return pair containing the first element of both bezier curve obtained.
   ///
-  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) {
+  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
     check_conditions();
     if (fabs(t - T_max_) < MARGIN) {
       throw std::runtime_error("can't split curve, interval range is equal to original curve");
@@ -319,6 +362,28 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     return std::make_pair(c_first, c_second);
   }
 
+  /// \brief Split the bezier curve in several curves, all accessible
+  /// within a piecewise_curve_t.
+  /// \param times : list of times of size n.
+  /// \return a piecewise_curve_t comprising n+1 curves
+  ///
+  piecewise_curve_t split(const vector_x_t& times) const {
+    std::vector<bezier_curve_t> curves;
+    bezier_curve_t current = *this;
+    for (int i = 0; i < times.rows(); ++i) {
+      std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
+      curves.push_back(pairsplit.first);
+      current = pairsplit.second;
+    }
+    curves.push_back(current);
+    piecewise_curve_t res;
+    for (typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end(); ++cit) {
+      typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
+      res.add_curve_ptr(ptr);
+    }
+    return res;
+  }
+
   /// \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.
@@ -392,6 +457,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   /// \brief Get the maximum time for which the curve is defined.
   /// \return \f$t_{max}\f$, upper bound of time range.
   virtual time_t max() const { return T_max_; }
+  /// \brief Get the degree of the curve.
+  /// \return \f$degree\f$, the degree of the curve.
+  virtual std::size_t degree() const { return degree_; }
   /*Helpers*/
 
   /* Attributes */
@@ -423,6 +491,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
     if (version) {
       // Do something depending on version ?
     }
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
     ar& boost::serialization::make_nvp("dim", dim_);
     ar& boost::serialization::make_nvp("T_min", T_min_);
     ar& boost::serialization::make_nvp("T_max", T_max_);

include/curves/cubic_hermite_spline.h

@@ -33,10 +33,13 @@ namespace curves {
 template <typename Time = double, typename Numeric = Time, bool Safe = false,
           typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
 struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
+  typedef Point point_t;
   typedef std::pair<Point, Point> pair_point_tangent_t;
   typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
   typedef std::vector<Time> vector_time_t;
   typedef Numeric num_t;
+  typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t;  // parent class
+  typedef cubic_hermite_spline<Time, Numeric, Safe, point_t> cubic_hermite_spline_t;
 
  public:
   /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
@@ -99,6 +102,41 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     }
   }
 
+  /**
+   * @brief isApprox check if other and *this are approximately equals.
+   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see
+   * isEquivalent
+   * @param other the other curve to check
+   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
+   * @return true is the two curves are approximately equals
+   */
+  bool isApprox(const cubic_hermite_spline_t& other,
+                const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
+    bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
+                 dim_ == other.dim() && degree_ == other.degree() && size_ == other.size() &&
+                 time_control_points_ == other.time_control_points_ && duration_splines_ == other.duration_splines_;
+    if (!equal) return false;
+    for (std::size_t i = 0; i < size_; ++i) {
+      if ((!control_points_[i].first.isApprox(other.control_points_[i].first, prec)) ||
+          (!control_points_[i].second.isApprox(other.control_points_[i].second, prec)))
+        return false;
+    }
+    return true;
+  }
+
+  virtual bool isApprox(const curve_abc_t* other,
+                        const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
+    const cubic_hermite_spline_t* other_cast = dynamic_cast<const cubic_hermite_spline_t*>(other);
+    if (other_cast)
+      return isApprox(*other_cast, prec);
+    else
+      return false;
+  }
+
+  virtual bool operator==(const cubic_hermite_spline_t& other) const { return isApprox(other); }
+
+  virtual bool operator!=(const cubic_hermite_spline_t& other) const { return !(*this == other); }
+
   ///  \brief Evaluate the derivative of order N of spline at time t.
   ///  \param t : time when to evaluate the spline.
   ///  \param order : order of derivative.
@@ -109,6 +147,17 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     return evalCubicHermiteSpline(t, order);
   }
 
+  cubic_hermite_spline_t compute_derivate(const std::size_t /*order*/) const {
+    throw std::logic_error("Compute derivate for cubic hermite spline is not implemented yet.");
+  }
+
+  ///  \brief Compute the derived curve at order N.
+  ///  \param order : order of derivative.
+  ///  \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+  cubic_hermite_spline_t* compute_derivate_ptr(const std::size_t order) const {
+    return new cubic_hermite_spline_t(compute_derivate(order));
+  }
+
   /// \brief Set time of each control point of cubic hermite spline.
   /// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with
   /// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
@@ -322,6 +371,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
   /// \brief Get the maximum time for which the curve is defined.
   /// \return \f$t_{max}\f$, upper bound of time range.
   Time virtual max() const { return time_control_points_.back(); }
+  /// \brief Get the degree of the curve.
+  /// \return \f$degree\f$, the degree of the curve.
+  virtual std::size_t degree() const { return degree_; }
   /*Helpers*/
 
   /*Attributes*/
@@ -356,6 +408,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     if (version) {
       // Do something depending on version ?
     }
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
     ar& boost::serialization::make_nvp("dim", dim_);
     ar& boost::serialization::make_nvp("control_points", control_points_);
     ar& boost::serialization::make_nvp("time_control_points", time_control_points_);
@@ -367,4 +420,4 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
   }
 };  // End struct Cubic hermite spline
 }  // namespace curves
-#endif  //_CLASS_CUBICHERMITESPLINE
\ No newline at end of file
+#endif  //_CLASS_CUBICHERMITESPLINE

include/curves/curve_abc.h

@@ -14,18 +14,30 @@
 #include "MathDefs.h"
 #include "serialization/archive.hpp"
 #include "serialization/eigen-matrix.hpp"
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/smart_ptr/shared_ptr.hpp>
 
 #include <functional>
 
 namespace curves {
+
+template <typename T>
+bool isApprox(const T a, const T b, const T eps = 1e-6) {
+  return fabs(a - b) < eps;
+}
+
 /// \struct curve_abc.
 /// \brief Represents a curve of dimension Dim.
 /// If value of parameter Safe is false, no verification is made on the evaluation of the curve.
 template <typename Time = double, typename Numeric = Time, bool Safe = false,
-          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
+          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename Point_derivate = Point>
 struct curve_abc : std::unary_function<Time, Point>, public serialization::Serializable {
   typedef Point point_t;
+  typedef Point_derivate 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 boost::shared_ptr<curve_t> curve_ptr_t;
 
   /* Constructors - destructors */
  public:
@@ -42,11 +54,65 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
   ///  \return \f$x(t)\f$, point corresponding on curve at time t.
   virtual point_t operator()(const time_t t) const = 0;
 
+  ///  \brief Compute the derived curve at order N.
+  ///  \param order : order of derivative.
+  ///  \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+  virtual curve_t* compute_derivate_ptr(const std::size_t order) const = 0;
+
   /// \brief Evaluate the derivative of order N of curve at time t.
   /// \param t : time when to evaluate the spline.
   /// \param order : order of derivative.
   /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
-  virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
+  virtual point_derivate_t derivate(const time_t t, const std::size_t order) const = 0;
+
+  /**
+   * @brief isEquivalent check if other and *this are approximately equal by values, given a precision treshold.
+   * This test is done by discretizing both curves and evaluating them and their derivatives.
+   * @param other the other curve to check
+   * @param order the order up to which the derivatives of the curves are checked for equality
+   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
+   * @return true is the two curves are approximately equals
+   */
+  bool isEquivalent(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),
+                    const size_t order = 5) const {
+    bool equal = curves::isApprox<num_t>(min(), other->min()) && curves::isApprox<num_t>(max(), other->max()) &&
+                 (dim() == other->dim());
+    if (!equal) {
+      return false;
+    }
+    time_t inc = (max() - min()) / 10.;  // FIXME : define this step somewhere ??
+    // check the value along the two curves
+    time_t t = min();
+    while (t <= max()) {
+      if (!(*this)(t).isApprox(other->operator()(t), prec)) {
+        return false;
+      }
+      t += inc;
+    }
+    //  check if the derivatives are equals
+    for (size_t n = 1; n <= order; ++n) {
+      t = min();
+      while (t <= max()) {
+        if (!derivate(t, n).isApprox(other->derivate(t, n), prec)) {
+          return false;
+        }
+        t += inc;
+      }
+    }
+    return true;
+  }
+
+  /**
+   * @brief isApprox check if other and *this are approximately equals given a precision treshold
+   * Only two curves of the same class can be approximately equals,
+   * for comparison between different type of curves see isEquivalent.
+   * @param other the other curve to check
+   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
+   * @return true is the two curves are approximately equals
+   */
+  virtual bool isApprox(const curve_t* other,
+                        const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const = 0;
+
   /*Operations*/
 
   /*Helpers*/
@@ -59,6 +125,10 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
   /// \brief Get the maximum time for which the curve is defined.
   /// \return \f$t_{max}\f$, upper bound of time range.
   virtual time_t max() const = 0;
+  /// \brief Get the degree of the curve.
+  /// \return \f$degree\f$, the degree of the curve.
+  virtual std::size_t degree() const = 0;
+
   std::pair<time_t, time_t> timeRange() { return std::make_pair(min(), max()); }
   /*Helpers*/
 
@@ -71,5 +141,6 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
     }
   }
 };
+BOOST_SERIALIZATION_ASSUME_ABSTRACT(curve_abc)
 }  // namespace curves
 #endif  //_STRUCT_CURVE_ABC

include/curves/curve_constraint.h

@@ -20,14 +20,32 @@ namespace curves {
 template <typename Point>
 struct curve_constraints {
   typedef Point point_t;
-  curve_constraints() {}
+  curve_constraints(const size_t dim = 3)
+      : init_vel(point_t::Zero(dim)),
+        init_acc(point_t::Zero(dim)),
+        init_jerk(point_t::Zero(dim)),
+        end_vel(point_t::Zero(dim)),
+        end_acc(point_t::Zero(dim)),
+        end_jerk(point_t::Zero(dim)),
+        dim_(dim) {}
 
-  ~curve_constraints() {}
+  curve_constraints(const curve_constraints& other)
+      : init_vel(other.init_vel),
+        init_acc(other.init_acc),
+        init_jerk(other.init_jerk),
+        end_vel(other.end_vel),
+        end_acc(other.end_acc),
+        end_jerk(other.end_jerk),
+        dim_(other.dim_) {}
 
+  ~curve_constraints() {}
   point_t init_vel;
   point_t init_acc;
+  point_t init_jerk;
   point_t end_vel;
   point_t end_acc;
+  point_t end_jerk;
+  size_t dim_;
 };
 }  // namespace curves
 #endif  //_CLASS_CUBICZEROVELACC

include/curves/curve_conversion.h

@@ -16,40 +16,37 @@ namespace curves {
 /// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
 /// \param curve   : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert.
 /// \return the equivalent polynomial.
-template <typename Polynomial, typename curveTypeToConvert>
-Polynomial polynomial_from_curve(const curveTypeToConvert& curve) {
+template <typename Polynomial>
+Polynomial polynomial_from_curve(const typename Polynomial::curve_abc_t& curve) {
   typedef typename Polynomial::t_point_t t_point_t;
   typedef typename Polynomial::num_t num_t;
   t_point_t coefficients;
-  curveTypeToConvert current(curve);
   coefficients.push_back(curve(curve.min()));
-  num_t T = curve.max() - curve.min();
-  num_t T_div = 1.0;
   num_t fact = 1;
-  for (std::size_t i = 1; i <= curve.degree_; ++i) {
+  for (std::size_t i = 1; i <= curve.degree(); ++i) {
     fact *= (num_t)i;
-    coefficients.push_back(current.derivate(current.min(), i) / fact);
+    coefficients.push_back(curve.derivate(curve.min(), i) / fact);