Merge branch 'devel' into release/1.4.0

CHANGELOG.md

@@ -7,6 +7,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+- Add a SO3 curves which is C2. It's called S03Smooth
+- Add some typedef on using 3D curves.
+- Fix typos around the package
+- Add some tests on polynomials and the new structs.
 - update tooling
 - update packaging
 - add pip CI

CMakeLists.txt

@@ -127,7 +127,8 @@ set(${PROJECT_NAME}_HEADERS
     include/${PROJECT_NAME}/serialization/eigen-matrix.hpp
     include/${PROJECT_NAME}/serialization/registeration.hpp
     include/${PROJECT_NAME}/sinusoidal.h
-    include/${PROJECT_NAME}/so3_linear.h)
+    include/${PROJECT_NAME}/so3_linear.h
+    include/${PROJECT_NAME}/so3_smooth.h)
 
 add_library(${PROJECT_NAME} INTERFACE)
 modernize_target_link_libraries(

include/ndcurves/bezier_curve.h

@@ -159,7 +159,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
    * 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true if the two curves are approximately equals
    */
@@ -633,7 +633,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
 
  private:
   /// \brief Ensure constraints of bezier curve.
-  /// Add 4 points (2 after the first one, 2 before the last one) to biezer
+  /// Add 4 points (2 after the first one, 2 before the last one) to Bezier
   /// curve to ensure that velocity and acceleration constraints are respected.
   ///
   template <typename In>

include/ndcurves/constant_curve.h

@@ -122,10 +122,10 @@ struct constant_curve
 
   /**
    * @brief isApprox check if other and *this are approximately equals given a
-   * precision treshold Only two curves of the same class can be approximately
+   * precision threshold 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */

include/ndcurves/cubic_hermite_spline.h

@@ -124,7 +124,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
    * 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */

include/ndcurves/curve_abc.h

@@ -77,12 +77,12 @@ struct curve_abc : public serialization::Serializable {
 
   /**
    * @brief isEquivalent check if other and *this are approximately equal by
-   * values, given a precision treshold. This test is done by discretizing both
+   * values, given a precision threshold. 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true if the two curves are approximately equal
    */
@@ -121,10 +121,10 @@ struct curve_abc : public serialization::Serializable {
 
   /**
    * @brief isApprox check if other and *this are approximately equal given a
-   * precision treshold Only two curves of the same class can be approximately
+   * precision threshold Only two curves of the same class can be approximately
    * equal, for comparison between different type of curves see isEquivalent.
    * @param other the other curve to check
-   * @param prec the precision treshold, default
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true if the two curves are approximately equal
    */

include/ndcurves/fwd.h

@@ -50,6 +50,9 @@ struct sinusoidal;
 template <typename Time, typename Numeric, bool Safe>
 struct SO3Linear;
 
+template <typename Time, typename Numeric, bool Safe>
+struct SO3Smooth;
+
 template <typename Numeric>
 struct Bern;
 
@@ -64,21 +67,25 @@ struct quadratic_variable;
 
 // typedef of the commonly used templates arguments :
 // eigen types :
+typedef Eigen::Matrix<double, 1, 1> point1_t;
 typedef Eigen::Vector3d point3_t;
 typedef Eigen::Matrix<double, 6, 1> point6_t;
 typedef Eigen::VectorXd pointX_t;
 typedef Eigen::Matrix<double, 3, 3> matrix3_t;
 typedef Eigen::Matrix<double, 4, 4> matrix4_t;
 typedef Eigen::Quaternion<double> quaternion_t;
 typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
-typedef std::vector<point3_t, Eigen::aligned_allocator<point3_t> > t_point3_t;
-typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
+typedef std::vector<point1_t, Eigen::aligned_allocator<point1_t>> t_point1_t;
+typedef std::vector<point3_t, Eigen::aligned_allocator<point3_t>> t_point3_t;
+typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t>> t_pointX_t;
+typedef Eigen::Ref<const matrix3_t> matrix3_t_cst_ref;
 
 // abstract curves types:
 typedef curve_abc<double, double, true, pointX_t, pointX_t>
     curve_abc_t;  // base abstract class
 typedef curve_abc<double, double, true, point3_t, point3_t>
-    curve_3_t;  // generic class of curve of size 3
+    curve_3_t;                          // generic class of curve of size 3
+typedef curve_3_t curve_translation_t;  // generic class of a translation curve
 typedef curve_abc<double, double, true, matrix3_t, point3_t>
     curve_rotation_t;  // templated class used for the rotation (return
                        // dimension are fixed)
@@ -90,6 +97,7 @@ typedef curve_abc<double, double, true, transform_t, point6_t>
 typedef boost::shared_ptr<curve_abc_t> curve_ptr_t;
 typedef boost::shared_ptr<curve_3_t> curve3_ptr_t;
 typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
+typedef boost::shared_ptr<curve_translation_t> curve_translation_ptr_t;
 typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
 
 // definition of all curves class with pointX as return type:
@@ -109,6 +117,7 @@ typedef sinusoidal<double, double, true, pointX_t> sinusoidal_t;
 
 // definition of all curves class with point3 as return type:
 typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
+typedef polynomial<double, double, true, point1_t, t_point1_t> polynomial1_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;
@@ -119,6 +128,7 @@ typedef piecewise_curve<double, double, true, point3_t, point3_t, curve_3_t>
     piecewise3_t;
 
 // special curves with return type fixed:
+typedef SO3Smooth<double, double, true> SO3Smooth_t;
 typedef SO3Linear<double, double, true> SO3Linear_t;
 typedef SE3Curve<double, double, true> SE3Curve_t;
 typedef piecewise_curve<double, double, true, transform_t, point6_t,

include/ndcurves/helpers/effector_spline_rotation.h

@@ -71,7 +71,7 @@ class rotation_spline : public curve_abc_quat_t {
     if (t >= max()) return quat_to_.coeffs();
     // normalize u
     Numeric u = (t - min()) / (max() - min());
-    // reparametrize u
+    // re-parametrize u
     return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
   }
 
@@ -80,7 +80,7 @@ class rotation_spline : public curve_abc_quat_t {
    * 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
@@ -127,7 +127,7 @@ class rotation_spline : public curve_abc_quat_t {
         "Compute derivate for quaternion spline is not implemented yet.");
   }
 
-  /// \brief Initialize time reparametrization for spline.
+  /// \brief Initialize time re-parametrization for spline.
   exact_cubic_constraint_one_dim computeWayPoints() const {
     t_waypoint_one_dim_t waypoints;
     waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));

include/ndcurves/linear_variable.h

@@ -210,8 +210,8 @@ struct linear_variable : public serialization::Serializable {
   Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
 
   /// \brief Check if actual linear variable and other are approximately equal
-  /// given a precision treshold. Only two curves of the same class can be
-  /// approximately equal, \param prec : the precision treshold, default
+  /// given a precision threshold. Only two curves of the same class can be
+  /// approximately equal, \param prec : the precision threshold, default
   /// Eigen::NumTraits<Numeric>::dummy_precision() \return true if the two
   /// linear variables are approximately equal.
   bool isApprox(
@@ -237,6 +237,19 @@ struct linear_variable : public serialization::Serializable {
     ar& boost::serialization::make_nvp("zero", zero);
   }
 
+  linear_variable& operator=(const linear_variable& other) {
+    if (this == &other) {
+      return *this;
+    }
+    // Perform a deep copy here to copy all necessary data.
+    // Make sure to handle memory allocation properly.
+    // You may need to copy the data contained within the linear_variable.
+    this->B_ = other.B_;
+    this->c_ = other.c_;
+    this->zero = other.zero;
+    return *this;
+  }
+
  private:
   matrix_x_t B_;
   vector_x_t c_;

include/ndcurves/optimization/details.h

@@ -235,17 +235,18 @@ split(const problem_definition<Point, Numeric>& pDef,
 
   const Eigen::VectorXd& times = pDef.splitTimes_;
   T_bezier_t res;
-  bezier_t& current = *pData.bezier;
+  bezier_t* current = pData.bezier;
   Numeric current_time = 0.;
   Numeric tmp;
   for (int i = 0; i < times.rows(); ++i) {
     tmp = times[i];
-    std::pair<bezier_t, bezier_t> pairsplit = current.split(tmp - current_time);
+    std::pair<bezier_t, bezier_t> pairsplit =
+        current->split(tmp - current_time);
     res.push_back(pairsplit.first);
-    current = pairsplit.second;
+    current = &(pairsplit.second);
     current_time += tmp - current_time;
   }
-  res.push_back(current);
+  res.push_back(*current);
   return res;
 }
 
@@ -350,34 +351,34 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1,
 }
 
 inline constraint_flag operator~(constraint_flag a) {
-  return static_cast<constraint_flag>(~static_cast<const int>(a));
+  return static_cast<constraint_flag>(~static_cast<int>(a));
 }
 
 inline constraint_flag operator|(constraint_flag a, constraint_flag b) {
-  return static_cast<constraint_flag>(static_cast<const int>(a) |
-                                      static_cast<const int>(b));
+  return static_cast<constraint_flag>(static_cast<int>(a) |
+                                      static_cast<int>(b));
 }
 
 inline constraint_flag operator&(constraint_flag a, constraint_flag b) {
-  return static_cast<constraint_flag>(static_cast<const int>(a) &
-                                      static_cast<const int>(b));
+  return static_cast<constraint_flag>(static_cast<int>(a) &
+                                      static_cast<int>(b));
 }
 
 inline constraint_flag operator^(constraint_flag a, constraint_flag b) {
-  return static_cast<constraint_flag>(static_cast<const int>(a) ^
-                                      static_cast<const int>(b));
+  return static_cast<constraint_flag>(static_cast<int>(a) ^
+                                      static_cast<int>(b));
 }
 
 inline constraint_flag& operator|=(constraint_flag& a, constraint_flag b) {
-  return (constraint_flag&)((int&)(a) |= static_cast<const int>(b));
+  return (constraint_flag&)((int&)(a) |= static_cast<int>(b));
 }
 
 inline constraint_flag& operator&=(constraint_flag& a, constraint_flag b) {
-  return (constraint_flag&)((int&)(a) &= static_cast<const int>(b));
+  return (constraint_flag&)((int&)(a) &= static_cast<int>(b));
 }
 
 inline constraint_flag& operator^=(constraint_flag& a, constraint_flag b) {
-  return (constraint_flag&)((int&)(a) ^= static_cast<const int>(b));
+  return (constraint_flag&)((int&)(a) ^= static_cast<int>(b));
 }
 
 }  // namespace optimization

include/ndcurves/piecewise_curve.h

@@ -105,7 +105,7 @@ struct piecewise_curve
    * 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */

include/ndcurves/polynomial.h

@@ -48,7 +48,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   /// \brief Empty constructor. Curve obtained this way can not perform other
   /// class functions.
   ///
-  polynomial() : curve_abc_t(), dim_(0), T_min_(0), T_max_(0) {}
+  polynomial() : curve_abc_t(), dim_(0), degree_(0), T_min_(0), T_max_(1.0) {}
 
   /// \brief Constructor.
   /// \param coefficients : a reference to an Eigen matrix where each column is
@@ -260,6 +260,26 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
   static polynomial_t MinimumJerk(const point_t& p_init, const point_t& p_final,
                                   const time_t t_min = 0.,
                                   const time_t t_max = 1.) {
+    polynomial_t out =
+        polynomial(coeff_t::Zero(p_init.size(), 6), t_min, t_max);
+    MinimumJerk(out, p_init, p_final, t_min, t_max);
+    return out;
+  }
+
+  /**
+   * @brief MinimumJerk Build a polynomial curve connecting p_init to p_final
+   * minimizing the time integral of the squared jerk with a zero initial and
+   * final velocity and acceleration.
+   * @param out The output polynomial needs to be of the correct size.
+   * @param p_init the initial point
+   * @param p_final the final point
+   * @param t_min initial time
+   * @param t_max final time
+   * @return the polynomial curve
+   */
+  static void MinimumJerk(polynomial_t& out, const point_t& p_init,
+                          const point_t& p_final, const time_t t_min = 0.,
+                          const time_t t_max = 1.) {
     if (t_min > t_max)
       throw std::invalid_argument(
           "final time should be superior or equal to initial time.");
@@ -273,13 +293,18 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
     const double T4 = T3 * T;
     const double T5 = T4 * T;
 
-    coeff_t coeffs =
-        coeff_t::Zero(dim, 6);  // init coefficient matrix with the right size
-    coeffs.col(0) = p_init;
-    coeffs.col(3) = 10 * (p_final - p_init) / T3;
-    coeffs.col(4) = -15 * (p_final - p_init) / T4;
-    coeffs.col(5) = 6 * (p_final - p_init) / T5;
-    return polynomial_t(coeffs, t_min, t_max);
+    assert(out.coefficients_.cols() == 6);
+    assert(out.coefficients_.rows() == static_cast<Eigen::Index>(dim));
+    assert(out.dim_ == dim);
+    out.coefficients_.fill(0.0);
+    out.coefficients_.col(0) = p_init;
+    out.coefficients_.col(3) = 10 * (p_final - p_init) / T3;
+    out.coefficients_.col(4) = -15 * (p_final - p_init) / T4;
+    out.coefficients_.col(5) = 6 * (p_final - p_init) / T5;
+    out.degree_ = 5;
+    out.T_min_ = t_min;
+    out.T_max_ = t_max;
+    out.safe_check();
   }
 
  private:
@@ -321,7 +346,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
    * 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
+   * @param prec the precision threshold, default
    * Eigen::NumTraits<Numeric>::dummy_precision()
    * @return true is the two curves are approximately equals
    */
@@ -686,4 +711,5 @@ DEFINE_CLASS_TEMPLATE_VERSION(
     SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
                typename T_Point),
     SINGLE_ARG(ndcurves::polynomial<Time, Numeric, Safe, Point, T_Point>))
+
 #endif  //_STRUCT_POLYNOMIAL