diff --git a/CMakeLists.txt b/CMakeLists.txt
index dcc5d52a56942cfedfcda0a86166a43c59beb7cc..a9b718fbf1b7ac906491360481617056f3f9fa1d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -68,6 +68,7 @@ SET(${PROJECT_NAME}_HEADERS
include/${PROJECT_NAME}/so3_linear.h
include/${PROJECT_NAME}/se3_curve.h
include/${PROJECT_NAME}/fwd.h
+ include/${PROJECT_NAME}/constant_curve.h
include/${PROJECT_NAME}/helpers/effector_spline.h
include/${PROJECT_NAME}/helpers/effector_spline_rotation.h
include/${PROJECT_NAME}/optimization/definitions.h
diff --git a/include/curves/constant_curve.h b/include/curves/constant_curve.h
new file mode 100644
index 0000000000000000000000000000000000000000..d67d1e2d04a8f90be899089b814973368486d5b6
--- /dev/null
+++ b/include/curves/constant_curve.h
@@ -0,0 +1,188 @@
+/**
+ * \file constant_curve.h
+ * \brief class allowing to create a constant_curve curve.
+ * \author Pierre Fernbach
+ * \version 0.4
+ * \date 29/04/2020
+ */
+
+#ifndef _CLASS_CONSTANTCURVE
+#define _CLASS_CONSTANTCURVE
+
+#include "curve_abc.h"
+
+
+
+namespace curves {
+/// \class constant_curve.
+/// \brief Represents a constant_curve curve, always returning the same value and a null derivative
+///
+template <typename Time = double, typename Numeric = Time, bool Safe = false,
+ typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
+ typename Point_derivate = Point>
+struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
+ typedef Point point_t;
+ typedef Point_derivate point_derivate_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
+ typedef constant_curve<Time, Numeric, Safe, Point, Point_derivate> constant_curve_t;
+ typedef constant_curve<Time, Numeric, Safe, Point_derivate> curve_derivate_t;
+ typedef curve_abc<Time, Numeric, Safe, point_t, Point_derivate> curve_abc_t; // parent class
+
+ /* Constructors - destructors */
+public:
+ /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
+ ///
+ constant_curve() : T_min_(0), T_max_(0), dim_(0) {}
+
+ /// \brief Constructor..
+ /// \param value : The constant value
+ /// \param T_min : lower bound of the time interval
+ /// \param T_max : upper bound of the time interval
+ ///
+ constant_curve(const Point& value, const time_t T_min = 0., const time_t T_max = std::numeric_limits<time_t>::max())
+ : value_(value),
+ T_min_(T_min),
+ T_max_(T_max),
+ dim_(value.size())
+ {
+ if(Safe && T_min_ > T_max_){
+ throw std::invalid_argument("can't create constant curve: min bound is higher than max bound");
+ }
+ }
+
+ /// \brief Copy constructor
+ /// \param other
+ constant_curve(const constant_curve_t& other)
+ : value_(other.value_),
+ T_min_(other.T_min_),
+ T_max_(other.T_max_),
+ dim_(other.dim_){}
+
+
+ /// \brief Destructor.
+ virtual ~constant_curve() {}
+ /* Constructors - destructors */
+
+ /*Operations*/
+ /// \brief Evaluation of the cubic spline at time t.
+ /// \param t : time when to evaluate the spine
+ /// \return \f$x(t)\f$, point corresponding on curve at time t.
+ virtual point_t operator()(const time_t t) const{
+ if (Safe && (t < T_min_ || t > T_max_)) {
+ throw std::invalid_argument(
+ "error in constant curve : time t to evaluate should be in range [Tmin, Tmax] of the curve");
+ }
+ return value_;
+ }
+
+
+ /// \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.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ curve_derivate_t compute_derivate() const {
+ size_t derivate_size;
+ if(point_derivate_t::RowsAtCompileTime == Eigen::Dynamic){
+ derivate_size = dim_;
+ }else{
+ derivate_size = point_derivate_t::RowsAtCompileTime;
+ }
+ point_derivate_t value(point_derivate_t::Zero(derivate_size));
+ return curve_derivate_t(value, T_min_, T_max_);
+ }
+
+ /// \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_derivate_t* compute_derivate_ptr(const std::size_t) const{
+ return new curve_derivate_t(compute_derivate());
+ }
+
+ /// \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_derivate_t derivate(const time_t t, const std::size_t) const{
+ if (Safe && (t < T_min_ || t > T_max_)) {
+ throw std::invalid_argument(
+ "error in constant curve : time t to derivate should be in range [Tmin, Tmax] of the curve");
+ }
+ size_t derivate_size;
+ if(point_derivate_t::RowsAtCompileTime == Eigen::Dynamic){
+ derivate_size = dim_;
+ }else{
+ derivate_size = point_derivate_t::RowsAtCompileTime;
+ }
+ return point_derivate_t::Zero(derivate_size);
+ }
+
+ /**
+ * @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 constant_curve_t& other,
+ const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+ return curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
+ dim_ == other.dim() && value_.isApprox(other.value_, prec);
+ }
+
+ virtual bool isApprox(const curve_abc_t* other,
+ const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
+ const constant_curve_t* other_cast = dynamic_cast<const constant_curve_t*>(other);
+ if (other_cast)
+ return isApprox(*other_cast, prec);
+ else
+ return false;
+ }
+
+ virtual bool operator==(const constant_curve_t& other) const { return isApprox(other); }
+
+ virtual bool operator!=(const constant_curve_t& other) const { return !(*this == other); }
+
+
+ /*Helpers*/
+ /// \brief Get dimension of curve.
+ /// \return dimension of curve.
+ std::size_t virtual dim() const { return dim_; }
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$ lower bound of time range.
+ num_t virtual min() const { return T_min_; }
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$ upper bound of time range.
+ num_t virtual 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 0; }
+ /*Helpers*/
+
+ /*Attributes*/
+ Point value_;
+ time_t T_min_, T_max_; // const
+ std::size_t dim_; // const
+ /*Attributes*/
+
+ // Serialization of the class
+ friend class boost::serialization::access;
+
+ template <class Archive>
+ void serialize(Archive& ar, const unsigned int version) {
+ if (version) {
+ // Do something depending on version ?
+ }
+ ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
+ ar& boost::serialization::make_nvp("value", value_);
+ ar& boost::serialization::make_nvp("T_min", T_min_);
+ ar& boost::serialization::make_nvp("T_max", T_max_);
+ ar& boost::serialization::make_nvp("dim", dim_);
+ }
+
+
+}; // struct constant_curve
+}//namespace curve
+
+#endif // _CLASS_CONSTANTCURVE
diff --git a/include/curves/fwd.h b/include/curves/fwd.h
index defc11a56d65936fa69f5dc66b9d21ead76995b0..2dd8d00920d8c11784aa7f19c25e203f7bbafab1 100644
--- a/include/curves/fwd.h
+++ b/include/curves/fwd.h
@@ -21,6 +21,9 @@ struct curve_abc;
template <typename Time, typename Numeric, bool Safe, typename Point>
struct bezier_curve;
+template <typename Time, typename Numeric, bool Safe, typename Point,typename Point_derivate>
+struct constant_curve;
+
template <typename Time, typename Numeric, bool Safe, typename Point>
struct cubic_hermite_spline;
@@ -81,6 +84,7 @@ typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
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 constant_curve<double, double, true, pointX_t, pointX_t> constant_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve<double, double, true, pointX_t, pointX_t, curve_abc_t> piecewise_t;
@@ -88,6 +92,7 @@ typedef piecewise_curve<double, double, true, pointX_t, pointX_t, curve_abc_t> p
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 constant_curve<double, double, true, point3_t, point3_t> constant3_t;
typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
typedef piecewise_curve<double, double, true, point3_t, point3_t, curve_3_t> piecewise3_t;
diff --git a/include/curves/serialization/curves.hpp b/include/curves/serialization/curves.hpp
index ff069a6c823ca42456b66687a2b9d3ccc7d71ff4..6275293010191cc4db69aad7f63e1468fbc73b02 100644
--- a/include/curves/serialization/curves.hpp
+++ b/include/curves/serialization/curves.hpp
@@ -18,6 +18,7 @@
#include "curves/se3_curve.h"
#include "curves/polynomial.h"
#include "curves/bezier_curve.h"
+#include "curves/constant_curve.h"
#include "curves/piecewise_curve.h"
#include "curves/exact_cubic.h"
#include "curves/cubic_hermite_spline.h"
diff --git a/include/curves/serialization/registeration.hpp b/include/curves/serialization/registeration.hpp
index b70f2e99d3177fd417fc746a7fbbaa197c3c63df..b7ad3b1fec30855050adf108e6cfa7335e3cf6bc 100644
--- a/include/curves/serialization/registeration.hpp
+++ b/include/curves/serialization/registeration.hpp
@@ -35,11 +35,13 @@ void register_types(Archive& ar) {
ar.template register_type<polynomial_t>();
ar.template register_type<exact_cubic_t>();
ar.template register_type<bezier_t>();
+ ar.template register_type<constant_t>();
ar.template register_type<cubic_hermite_spline_t>();
ar.template register_type<piecewise_t>();
ar.template register_type<polynomial3_t>();
ar.template register_type<exact_cubic3_t>();
ar.template register_type<bezier3_t>();
+ ar.template register_type<constant3_t>();
ar.template register_type<cubic_hermite_spline3_t>();
ar.template register_type<piecewise3_t>();
ar.template register_type<SO3Linear_t>();