Merge remote-tracking branch 'main/devel' into devel

include/curves/bernstein.h

@@ -41,7 +41,9 @@ struct Bern {
   ~Bern() {}
 
   Numeric operator()(const Numeric u) const {
-    assert(u >= 0. && u <= 1.);
+    if (!(u >= 0. && u <= 1.)) {
+      throw std::invalid_argument("u needs to be betwen 0 and 1.");
+    }
     return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
   }
 

include/curves/bezier_curve.h

@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
         size_(std::distance(PointsBegin, PointsEnd)),
         degree_(size_ - 1),
         bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_)) {
-    assert(bernstein_.size() == size_);
+    if (bernstein_.size() != size_) {
+      throw std::invalid_argument("Invalid size of polynomial");
+    }
     In it(PointsBegin);
     if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
-      throw std::invalid_argument("can't create bezier min bound is higher than max bound");  // TODO
+      throw std::invalid_argument("can't create bezier min bound is higher than max bound");
     }
     for (; it != PointsEnd; ++it) {
       control_points_.push_back(*it);

include/curves/cubic_hermite_spline.h

@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     //
     const Time dt = (t1 - t0);
     const Time alpha = (t - t0) / dt;
-    assert(0. <= alpha && alpha <= 1. && "alpha must be in [0,1]");
+    if (!(0. <= alpha && alpha <= 1.)) {
+      throw std::runtime_error("alpha must be in [0,1]");
+    }
     Numeric h00, h10, h01, h11;
     evalCoeffs(alpha, h00, h10, h01, h11, degree_derivative);
     // std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<"

include/curves/helpers/effector_spline.h

@@ -37,7 +37,9 @@ typedef exact_cubic_t::spline_t spline_t;
 /// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
 Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
   Numeric norm = normal.norm();
-  assert(norm > 0.);
+  if (norm < 0.) {
+    throw std::runtime_error("Norm of normal is less than 0!");
+  }
   return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
 }
 
@@ -46,7 +48,9 @@ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numer
 spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time,
                          const Time time_offset) {
   Numeric norm = normal.norm();
-  assert(norm > 0.);
+  if (norm < 0.) {
+    throw std::runtime_error("Norm of normal is less than 0!");
+  }
   Point n = normal / norm;
   Point d = offset / (time_offset * time_offset * time_offset) * -n;
   Point c = -3 * d * time_offset;

include/curves/optimization/OptimizeSpline.h

@@ -38,7 +38,9 @@ struct SplineOptimizer {
   ///\brief Initializes optimizer environment.
   SplineOptimizer() {
     MSKrescodee r_ = MSK_makeenv(&env_, NULL);
-    assert(r_ == MSK_RES_OK);
+    if (r_ != MSK_RES_OK) {
+      throw std::runtime_error("Issue initializing MSK_makeenv");
+    }
   }
 
   ///\brief Destructor.

include/curves/optimization/details.h

@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
   // add remaining variables (only if no end_pos constraints)
   for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
 
-  assert(numControlPoints > numConstants);
-  assert(numControlPoints == variables_.size());
+  if (numControlPoints <= numConstants) {
+    throw std::runtime_error("numControlPoints < numConstants");
+  }
+  if (numControlPoints != variables_.size()) {
+    throw std::runtime_error("numControlPoints != variables_.size()");
+  }
 
   problemData.numControlPoints = numControlPoints;
   problemData.numVariables = numControlPoints - numConstants;
@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
   // compute sub-bezier curves
   T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
 
-  assert(pDef.inequalityMatrices_.size() == pDef.inequalityVectors_.size());
-  assert(pDef.inequalityMatrices_.size() == beziers.size());
+  if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
+    throw std::invalid_argument("The sizes of the inequality matrices and vectors do not match.");
+  }
+  if (pDef.inequalityMatrices_.size() != beziers.size()) {
+    throw std::invalid_argument("The sizes of the inequality matrices and the bezier degree do not match.");
+  }
 
   long currentRowIdx = 0;
   typename problem_definition_t::CIT_matrix_x_t cmit = pDef.inequalityMatrices_.begin();
@@ -264,7 +272,7 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
       currentRowIdx += cmit->rows();
     }
   }
-  assert(rows == currentRowIdx);  // we filled all the constraints
+  assert(rows == currentRowIdx);  // we filled all the constraints - NB: leave assert for Debug tests
 }
 
 template <typename Point, typename Numeric, typename In>
@@ -273,8 +281,9 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In Po
   typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
   unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
                nPoints2 = (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
-  assert(nPoints1 > 0);
-  assert(nPoints2 > 0);
+  if (nPoints1 <= 0 || nPoints2 <= 0) {
+    throw std::runtime_error("This should never happen because an unsigned int cannot go negative without underflowing.");
+  }
   unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
   unsigned int newDeg = (deg1 + deg2);
   // the integral of the primitive will simply be the last control points of the primitive,

include/curves/piecewise_curve.h

@@ -212,9 +212,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
 
   std::size_t num_curves() const { return curves_.size(); }
 
-  const curve_ptr_t& curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
+  curve_ptr_t curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
 
-  const curve_ptr_t& curve_at_index(const std::size_t idx) const {
+  curve_ptr_t curve_at_index(const std::size_t idx) const {
     if (Safe && idx >= num_curves()) {
       throw std::length_error(
           "curve_at_index: requested index greater than number of curves in piecewise_curve instance");

include/curves/polynomial.h

@@ -106,6 +106,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   ///
   polynomial(const Point& init, const Point& end, const time_t min, const time_t max)
       : dim_(init.size()), degree_(1), T_min_(min), T_max_(max) {
+    if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
     if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
     t_point_t coeffs;
     coeffs.push_back(init);
@@ -127,6 +128,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   polynomial(const Point& init, const Point& d_init, const Point& end, const Point& d_end, const time_t min,
              const time_t max)
       : dim_(init.size()), degree_(3), T_min_(min), T_max_(max) {
+    if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
     if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
     if (init.size() != d_init.size())
       throw std::invalid_argument("init and d_init points must have the same dimensions.");
@@ -170,6 +172,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   polynomial(const Point& init, const Point& d_init, const Point& dd_init, const Point& end, const Point& d_end,
              const Point& dd_end, const time_t min, const time_t max)
       : dim_(init.size()), degree_(5), T_min_(min), T_max_(max) {
+    if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
     if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
     if (init.size() != d_init.size())
       throw std::invalid_argument("init and d_init points must have the same dimensions.");

include/curves/quadratic_variable.h

@@ -35,7 +35,9 @@ struct quadratic_variable {
   }
 
   quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0) : c_(c), b_(b), A_(A), zero(false) {
-    assert(A.cols() == b.rows() && A.cols() == A.rows());
+    if (A.cols() != b.rows() || A.cols() != A.rows()) {
+      throw std::invalid_argument("The dimensions of A and b are incorrect.");
+    }
   }
 
   quadratic_variable(const point_t& b, const Numeric c = 0)
@@ -45,7 +47,9 @@ struct quadratic_variable {
 
   // linear evaluation
   Numeric operator()(const Eigen::Ref<const point_t>& val) const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return val.transpose() * A() * val + b().transpose() * val + c();
   }
 
@@ -98,15 +102,21 @@ struct quadratic_variable {
   }
 
   const matrix_x_t& A() const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return A_;
   }
   const point_t& b() const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return b_;
   }
   const Numeric c() const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return c_;
   }
   bool isZero() const { return zero; }

include/curves/se3_curve.h

@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
   typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
   typedef transform_t point_t;
   typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
-  typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
-  typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
-  typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
   typedef Eigen::Quaternion<Scalar> Quaternion;
   typedef Time time_t;
   typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;  // parent class
@@ -215,6 +212,10 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
   /// \brief Get the degree of the curve.
   /// \return \f$degree\f$, the degree of the curve.
   virtual std::size_t degree() const { return translation_curve_->degree(); }
+  /// \brief const accessor to the translation curve
+  const curve_ptr_t translation_curve() const {return translation_curve_;}
+  /// \brief const accessor to the rotation curve
+  const curve_rotation_ptr_t rotation_curve() const {return rotation_curve_;}
   /*Helpers*/
 
   /*Attributes*/

include/curves/serialization/archive.hpp

@@ -59,7 +59,9 @@ struct Serializable {
   /// \brief Loads a Derived object from an XML file.
   template <class Derived>
   void loadFromXML(const std::string& filename, const std::string& tag_name) {
-    assert(!tag_name.empty());
+    if (tag_name.empty()) {
+      throw std::invalid_argument("tag_name cannot be empty.");
+    }
     std::ifstream ifs(filename.c_str());
     if (ifs) {
       boost::archive::xml_iarchive ia(ifs);
@@ -74,7 +76,9 @@ struct Serializable {
   /// \brief Saved a Derived object as an XML file.
   template <class Derived>
   void saveAsXML(const std::string& filename, const std::string& tag_name) const {
-    assert(!tag_name.empty());
+    if (tag_name.empty()) {
+      throw std::invalid_argument("tag_name cannot be empty.");
+    }
     std::ofstream ofs(filename.c_str());
     if (ofs) {
       boost::archive::xml_oarchive oa(ofs);

include/curves/so3_linear.h

@@ -17,10 +17,8 @@ namespace curves {
 ///
 ///
 template <typename Time = double, typename Numeric = Time, bool Safe = false>
-struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, 3, 3>, Eigen::Matrix<Numeric, 3, 1> > {
+struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
   typedef Numeric Scalar;
-  typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
-  typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
   typedef matrix3_t point_t;
   typedef point3_t point_derivate_t;
   typedef Eigen::Quaternion<Scalar> quaternion_t;
@@ -40,7 +38,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
         dim_(3),
         init_rot_(init_rot),
         end_rot_(end_rot),
-        angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix()) / (t_max - t_min)),
+        angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), t_min, t_max)),
         T_min_(t_min),
         T_max_(t_max) {
     safe_check();
@@ -52,7 +50,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
         dim_(3),
         init_rot_(quaternion_t(init_rot)),
         end_rot_(quaternion_t(end_rot)),
-        angular_vel_(log3(init_rot.transpose() * end_rot) / (t_max - t_min)),
+        angular_vel_(computeAngularVelocity(init_rot, end_rot, t_min, t_max)),
         T_min_(t_min),
         T_max_(t_max) {
     safe_check();
@@ -64,7 +62,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
         dim_(3),
         init_rot_(init_rot),
         end_rot_(end_rot),
-        angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())),
+        angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), 0., 1.)),
         T_min_(0.),
         T_max_(1.) {
     safe_check();
@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
         dim_(3),
         init_rot_(quaternion_t(init_rot)),
         end_rot_(quaternion_t(end_rot)),
-        angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())),
+        angular_vel_(computeAngularVelocity(init_rot, end_rot, 0., 1.)),
         T_min_(0.),
         T_max_(1.) {
     safe_check();
@@ -95,12 +93,20 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
         T_min_(other.T_min_),
         T_max_(other.T_max_) {}
 
+  point3_t computeAngularVelocity(const matrix3_t& init_rot, const matrix3_t& end_rot, const double t_min, const double t_max){
+    if(t_min == t_max){
+      return point3_t::Zero();
+    }else{
+      return log3(init_rot.transpose() * end_rot) / (t_max - t_min);
+    }
+  }
+
   quaternion_t computeAsQuaternion(const time_t t) const {
     if (Safe & !(T_min_ <= t && t <= T_max_)) {
       throw std::invalid_argument("can't evaluate bezier curve, time t is out of range");  // TODO
     }
-    if (t > T_max_) return end_rot_;
-    if (t < T_min_) return init_rot_;
+    if (t >= T_max_) return end_rot_;
+    if (t <= T_min_) return init_rot_;
     Scalar u = (t - T_min_) / (T_max_ - T_min_);
     return init_rot_.slerp(u, end_rot_);
   }
@@ -108,7 +114,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
   ///  \brief Evaluation of the SO3Linear at time t using Eigen slerp.
   ///  \param t : time when to evaluate the spline.
   ///  \return \f$x(t)\f$ point corresponding on spline at time t.
-  virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
+  virtual point_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
 
   /**
    * @brief isApprox check if other and *this are approximately equals.
@@ -141,7 +147,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
   ///  \param t : the time when to evaluate the spline.
   ///  \param order : order of derivative.
   ///  \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
-  virtual point3_t derivate(const time_t t, const std::size_t order) const {
+  virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
     if ((t < T_min_ || t > T_max_) && Safe) {
       throw std::invalid_argument(
           "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
@@ -250,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
       theta = PI_value;  // acos((-1-1)/2)
     else
       theta = acos((tr - Scalar(1)) / Scalar(2));
-    assert(theta == theta && "theta contains some NaN");  // theta != NaN
+    if (!std::isfinite(theta)) {
+      throw std::runtime_error("theta contains some NaN");
+    }
 
     // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
     if (theta < PI_value - 1e-2) {

python/curves/CMakeLists.txt

@@ -8,6 +8,15 @@ SET(${PY_NAME}_BINDINGS_SOURCES
   namespace.h
   )
 
+SET(${PROJECT_NAME}_PYTHON_HEADERS
+  python_definitions.h
+  )
+
+INSTALL(FILES
+  ${${PROJECT_NAME}_PYTHON_HEADERS}
+  DESTINATION include/${PROJECT_NAME}/python
+  )
+
 ADD_LIBRARY(${PY_NAME} SHARED ${${PY_NAME}_BINDINGS_SOURCES})
 SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES PREFIX "")
 TARGET_COMPILE_OPTIONS(${PY_NAME} PRIVATE "-Wno-conversion")

python/curves/python_definitions.h

 #include "curves/fwd.h"
 #include "curves/bezier_curve.h"
+#include "curves/polynomial.h"
 #include "curves/linear_variable.h"
 #include "curves/quadratic_variable.h"
 
@@ -25,8 +26,8 @@ typedef std::vector<waypoint_t> t_waypoint_t;
 typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> point_listX_t;
 typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list3_t;
 typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
-
 typedef polynomial_t::coeff_t coeff_t;
+
 typedef curves::Bern<double> bernstein_t;
 
 template <typename PointList, typename T_Point>
@@ -45,5 +46,16 @@ T_Point vectorFromEigenVector(const PointList& vector) {
   }
   return res;
 }
+
+template <typename T_point, typename PointList>
+PointList vectorToEigenArray(const T_point& vect) {
+  const size_t nCols = vect.size();
+  const size_t nRows = vect[0].rows();
+  PointList res(nRows,nCols);
+  for (size_t i = 0; i < vect.size(); ++i) {
+    res.block(0,i,nRows,1) = vect[i];
+  }
+  return res;
+}
 }  // namespace curves
 #endif  //_DEFINITION_PYTHON_BINDINGS

python/curves/python_variables.cpp

@@ -5,7 +5,9 @@
 
 namespace curves {
 std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list3_t& matrices, const point_list3_t& vectors) {
-  assert(vectors.cols() * 3 == matrices.cols());
+  if (vectors.cols() * 3 != matrices.cols()) {
+    throw std::invalid_argument("vectors.cols() * 3 != matrices.cols()");
+  }
   std::vector<linear_variable_t> res;
   for (int i = 0; i < vectors.cols(); ++i) {
     res.push_back(linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i)));

python/test/test.py

@@ -98,6 +98,14 @@ class TestCurves(unittest.TestCase):
             self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
             self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
         self.assertTrue((prim(0) == array([0., 0., 0.])).all())
+        # testing accessor to waypoints :
+        wp_getter = a0.waypoints()
+        self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
+        self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
+        self.assertTrue(array_equal(wp_getter,waypoints))
+        # check that it return a copy:
+        a0.waypoints()[1,1] = -15.
+        self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
         # testing bezier with constraints
         c = curve_constraints(3)
         c.init_vel = array([[0., 1., 1.]]).transpose()
@@ -189,6 +197,14 @@ class TestCurves(unittest.TestCase):
             self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
             self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
         self.assertTrue((prim(0) == array([0., 0., 0.])).all())
+        # testing accessor to waypoints :
+        wp_getter = a0.waypoints()
+        self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
+        self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
+        self.assertTrue(array_equal(wp_getter,waypoints))
+        # check that it return a copy:
+        a0.waypoints()[1,1] = -15.
+        self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
         # testing bezier with constraints
         c = curve_constraints(3)
         c.init_vel = array([[0., 1., 1.]]).transpose()
@@ -344,6 +360,11 @@ class TestCurves(unittest.TestCase):
         pc.derivate(0.4, 2)
         pc.is_continuous(0)
         pc.is_continuous(1)
+        a0 = pc.curve_at_index(0)
+        self.assertTrue(array_equal(a0(0.5), pc(0.5)))
+        self.assertTrue(a0 == a)
+        a0 = b # should not have any effect
+        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
         # Test serialization
         pc.saveAsText("serialization_pc.test")
         pc_test = piecewise()
@@ -397,6 +418,13 @@ class TestCurves(unittest.TestCase):
         for i in range(N):
             self.assertTrue(isclose(polC0(time_points[i, 0]), points[:, i]).all())
 
+        c0 = polC0.curve_at_index(0)
+        self.assertEqual(c0.min(), time_points[0])
+        self.assertEqual(c0.max(), time_points[1])
+        self.assertEqual(c0.dim(), 3)
+        mid_t = (c0.max() + c0.min()) /2.
+        self.assertTrue(array_equal(polC0(mid_t), c0(mid_t)))
+
         polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
         self.assertEqual(polC1.min(), time_points[0, 0])
         self.assertEqual(polC1.max(), time_points[-1, 0])
@@ -448,6 +476,14 @@ class TestCurves(unittest.TestCase):
         pc.derivate(0.4, 2)
         pc.is_continuous(0)
         pc.is_continuous(1)
+        # test access to curves :
+        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
+        waypoints = array([[3., 4., -3.], [5., 1., 2.]]).transpose()
+        c = bezier(waypoints, 1.5, 2.)
+        c0 = pc.curve_at_index(0)
+        c0 = c # should not have any effect
+        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
+
         # Test serialization
         pc.saveAsText("serialization_pc.test")
         pc_test = piecewise()
@@ -476,6 +512,11 @@ class TestCurves(unittest.TestCase):
         pc.derivate(0.4, 2)
         pc.is_continuous(0)
         pc.is_continuous(1)
+        a0 = pc.curve_at_index(0)
+        self.assertTrue(array_equal(a0(0.5), pc(0.5)))
+        self.assertTrue(a0 == a)
+        a0 = b # should not have any effect
+        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
         # Test serialization
         pc.saveAsText("serialization_pc.test")
         pc_test = piecewise()
@@ -897,6 +938,18 @@ class TestCurves(unittest.TestCase):
         self.assertTrue(isclose(d, se3.derivate(max, 1)).all())
         self.assertTrue(isclose(se3.derivate(min, 2), zeros((6, 1))).all())
         self.assertTrue(isclose(se3.derivate(min, 3), zeros((6, 1))).all())
+
+        # test accessor to translation_curve :
+        tr_se3 = se3.translation_curve()
+        self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
+        # test accessor to rotation :
+        rot_se3 = se3.rotation_curve()
+        rot_se3((max+min)/2.)
+        self.assertTrue(isclose(rot_se3((max+min)/2.),(se3.rotation((max+min)/2.))).all())
+        # check that it return a CONST reference :
+        waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
+        tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
+        self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
         # check that errors are correctly raised when necessary :
         with self.assertRaises(ValueError):
             se3(0.)
@@ -949,6 +1002,21 @@ class TestCurves(unittest.TestCase):
             self.assertTrue(isclose(se3.derivate(t, 1)[0:3], translation.derivate(t, 1)).all())
             t += 0.02
 
+        # test accessor to translation_curve :
+        tr_se3 = se3.translation_curve()
+        self.assertTrue(tr_se3 == translation)
+        self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
+        # test accessor to rotation :
+        rot_se3 = se3.rotation_curve()
+        rot_se3((max+min)/2.)
+        se3.rotation((max+min)/2.)
+        self.assertTrue(isclose(rot_se3((max+min)/2.), se3.rotation((max+min)/2.)).all())
+        # check that it return a CONST reference :
+        waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
+        tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
+        self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
+
+
         # test with bezier3
         translation = bezier3(waypoints, min, max)
         se3 = SE3Curve(translation, init_rot, end_rot)
@@ -1051,6 +1119,20 @@ class TestCurves(unittest.TestCase):
             self.assertTrue(isclose(se3.derivate(t, 1)[3:6], rotation.derivate(t, 1)).all())
             t += 0.02
 
+        # test accessor to translation_curve :
+        tr_se3 = se3.translation_curve()
+        self.assertTrue(tr_se3 == translation)
+        self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
+        # test accessor to rotation :
+        rot_se3 = se3.rotation_curve()
+        rot_se3((max+min)/2.)
+        se3.rotation((max+min)/2.)
+        self.assertTrue(array_equal(rot_se3((max+min)/2.), se3.rotation((max+min)/2.)))
+        # check that it return a CONST reference :
+        waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
+        tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
+        self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
+
         # check if errors are correctly raised :
         rotation = SO3Linear(init_rot, end_rot, min + 0.2, max)
         with self.assertRaises(ValueError):

tests/Main.cpp

@@ -1855,6 +1855,36 @@ void se3CurveTest(bool& error) {
     std::cout << "SE3 curve : second order derivative for rotation should be null" << std::endl;
   }
 
+  // check accessor to translation curves :
+  curve_ptr_t translation = cBezier.translation_curve();
+  if (translation->operator()(min) != cBezier(min).translation()) {
+    error = true;
+    std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
+  }
+  if (translation->operator()(max) != cBezier(max).translation()) {
+    error = true;
+    std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
+  }
+  if (translation->operator()((max+min)/2.) != cBezier((max+min)/2.).translation()) {
+    error = true;
+    std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
+  }
+  // check accessor to rotation curves :
+  curve_rotation_ptr_t rotation = cBezier.rotation_curve();
+  if (! rotation->operator()(min).isApprox(cBezier(min).rotation())) {
+    error = true;
+    std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
+  }
+  if (! rotation->operator()(max).isApprox(cBezier(max).rotation())) {
+    error = true;
+    std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;