Commit f8af086b authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v0.3.0'

Release of version 0.3.0.
parents 4fcc8aef 11c34d91
......@@ -6,3 +6,4 @@ build-rel/
.*~
*.user
*.pyc
*.ipynb_checkpoints
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()
......@@ -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.
Subproject commit bbdade93ad2a4b7818cb5d5b9f0bf131627e0b71
Subproject commit e7892518eb62e1d20823307af48d6041f78a8da4
File added
This diff is collapsed.
......@@ -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)
......@@ -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_;
......
......@@ -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_);
......
......@@ -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
......@@ -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
......@@ -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
......@@ -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);
}