...
 
Commits (25)
......@@ -65,7 +65,9 @@ SET(${PROJECT_NAME}_HEADERS
include/${PROJECT_NAME}/piecewise_curve.h
include/${PROJECT_NAME}/so3_linear.h
include/${PROJECT_NAME}/se3_curve.h
include/${PROJECT_NAME}/sinusoidal.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
......
/**
* \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 curves
#endif // _CLASS_CONSTANTCURVE
......@@ -37,6 +37,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
typedef Time time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_t; // parent class
typedef curve_abc<Time, Numeric, Safe, point_derivate_t> curve_derivate_t; // parent class
typedef boost::shared_ptr<curve_t> curve_ptr_t;
/* Constructors - destructors */
......@@ -57,7 +58,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \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;
virtual curve_derivate_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.
......
......@@ -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;
......@@ -36,6 +39,9 @@ struct polynomial;
template <typename Time, typename Numeric, bool Safe>
struct SE3Curve;
template <typename Time, typename Numeric, bool Safe, typename Point>
struct sinusoidal;
template <typename Time, typename Numeric, bool Safe>
struct SO3Linear;
......@@ -81,13 +87,16 @@ 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;
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 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;
......
......@@ -12,6 +12,8 @@
#include "curve_conversion.h"
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <fstream>
#include <sstream>
namespace curves {
/// \class PiecewiseCurve.
......@@ -39,6 +41,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
typedef typename std::vector<Time> t_time_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, Point_derivate, CurveType> piecewise_curve_t;
typedef piecewise_curve<Time, Numeric, Safe, Point_derivate, Point_derivate,
typename CurveType::curve_derivate_t> piecewise_curve_derivate_t;
typedef boost::shared_ptr<typename piecewise_curve_derivate_t::curve_t> curve_derivate_ptr_t;
public:
/// \brief Empty constructor. Add at least one curve to call other class functions.
......@@ -124,10 +129,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
* @param order order of derivative
* @return
*/
piecewise_curve_t* compute_derivate_ptr(const std::size_t order) const {
piecewise_curve_t* res(new piecewise_curve_t());
piecewise_curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
piecewise_curve_derivate_t* res(new piecewise_curve_derivate_t());
for (typename t_curve_ptr_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
curve_ptr_t ptr((*itc)->compute_derivate_ptr(order));
curve_derivate_ptr_t ptr((*itc)->compute_derivate_ptr(order));
res->add_curve_ptr(ptr);
}
return res;
......@@ -359,6 +364,125 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return piecewise_res;
}
/**
* @brief load_piecewise_from_text_file build a piecewise polynomial from a list of discrete points read from a file.
* The file should contains one points per line, optionally with it's derivative and second derivatives.
* Each lines should then contains dim, 2*dim or 3*dim values
* @param filename the (absolute) name of the file to load
* @param dt the time step between each points in the file
* @param dim the dimension of the curve
* @return a piecewise curves containing polynomial connectiong all the points in the file
*/
template <typename Polynomial>
static piecewise_curve_t load_piecewise_from_text_file(const std::string& filename, const time_t dt, const size_t dim){
if(dim <= 0)
throw std::invalid_argument("The dimension should be strictly positive.");
if(dt <= 0.)
throw std::invalid_argument("The time step should be strictly positive.");
piecewise_curve_t piecewise_res;
std::ifstream file;
file.open(filename.c_str());
point_t last_pos = point_t::Zero(dim),
last_vel = point_t::Zero(dim),
last_acc = point_t::Zero(dim),
new_pos = point_t::Zero(dim),
new_vel = point_t::Zero(dim),
new_acc = point_t::Zero(dim);
bool use_vel, use_acc;
std::string line;
// read first line to found out if we use velocity / acceleration :
std::getline(file, line);
std::istringstream iss_length(line);
const size_t length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
if(length == dim){
use_vel = false;
use_acc = false;
}else if(length == dim * 2){
use_vel = true;
use_acc = false;
}else if(length == dim * 3){
use_vel = true;
use_acc = true;
}else{
std::stringstream error;
error<<"The first line of the file shold contains either " << dim << ", " << dim * 2 << " or " << dim * 3 <<
"values, got : " << length;
throw std::invalid_argument(error.str());
}
// initialize the first points of the trajectory:
num_t val;
std::istringstream iss(line);
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
last_pos[i] = val;
}
if(use_vel){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
last_vel[i] = val;
}
}
if(use_acc){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
last_acc[i] = val;
}
}
size_t current_length;
size_t line_id = 0;
// parse all lines of the file:
while (std::getline(file, line))
{
++line_id;
std::istringstream iss_length(line);
current_length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
if(current_length != length){
std::stringstream error;
error<<"Cannot parse line " << line_id << " got " << current_length << " values instead of " << length;
throw std::invalid_argument(error.str());
}
std::istringstream iss(line);
// parse the points values from the file:
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
new_pos[i] = val;
}
if(use_vel){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
new_vel[i] = val;
}
}
if(use_acc){
for(size_t i = 0 ; i < dim ; ++i){
iss >> val;
new_acc[i] = val;
}
}
// append a new curves connectiong this points
if(use_acc){
piecewise_res.add_curve(Polynomial(last_pos, last_vel, last_acc,
new_pos, new_vel, new_acc,
dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
}else if(use_vel){
piecewise_res.add_curve(Polynomial(last_pos, last_vel,
new_pos, new_vel,
dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
}else{
piecewise_res.add_curve(Polynomial(last_pos, new_pos,
dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
}
last_pos = new_pos;
last_vel = new_vel;
last_acc = new_acc;
}
file.close();
return piecewise_res;
}
private:
/// \brief Get index of the interval corresponding to time t for the interpolation.
/// \param t : time where to look for interval.
......
......@@ -224,6 +224,35 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
// polynomial& operator=(const polynomial& other);
/**
* @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 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 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.) {
if (t_min > t_max) throw std::invalid_argument("final time should be superior or equal to initial time.");
const size_t dim(p_init.size());
if (static_cast<size_t>(p_final.size()) != dim)
throw std::invalid_argument("Initial and final points must have the same dimension.");
const double T = t_max - t_min;
const double T2 = T * T;
const double T3 = T2 * T;
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);
}
private:
void safe_check() {
if (Safe) {
......
......@@ -28,6 +28,7 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
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
typedef polynomial<Time, Numeric, Safe, point_derivate_t> curve_derivate_t;
typedef curve_abc<Time, Numeric, Safe, pointX_t> curve_X_t; // generic class of curve
typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t>
curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
......@@ -190,14 +191,14 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
return res;
}
SE3Curve_t compute_derivate(const std::size_t /*order*/) const {
curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
throw std::logic_error("Compute derivate for SE3 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.
SE3Curve_t* compute_derivate_ptr(const std::size_t order) const { return new SE3Curve_t(compute_derivate(order)); }
curve_derivate_t* compute_derivate_ptr(const std::size_t order) const { return new curve_derivate_t(compute_derivate(order)); }
/*Helpers*/
/// \brief Get dimension of curve.
......
......@@ -16,8 +16,10 @@
#include "curves/curve_abc.h"
#include "curves/so3_linear.h"
#include "curves/se3_curve.h"
#include "curves/sinusoidal.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"
......
......@@ -35,15 +35,18 @@ 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>();
ar.template register_type<SE3Curve_t>();
ar.template register_type<sinusoidal_t>();
ar.template register_type<piecewise_SE3_t>();
}
......
/**
* \file sinusoidal.h
* \brief class allowing to create a sinusoidal curve.
* \author Pierre Fernbach
* \version 0.4
* \date 29/04/2020
*/
#ifndef _CLASS_SINUSOIDALCURVE
#define _CLASS_SINUSOIDALCURVE
#include "curve_abc.h"
#include <cmath>
namespace curves {
/// \class sinusoidal.
/// \brief Represents a sinusoidal curve, evaluating the following equation:
/// p0 + amplitude * (sin(2pi/T + phi)
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct sinusoidal : public curve_abc<Time, Numeric, Safe, Point> {
typedef Point point_t;
typedef Point point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
typedef sinusoidal<Time, Numeric, Safe, Point> sinusoidal_t;
typedef curve_abc<Time, Numeric, Safe, Point> curve_abc_t; // parent class
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
sinusoidal() : T_min_(0), T_max_(0), dim_(0) {}
/// \brief Constructor
/// \param p0 : Offset of the sinusoidal
/// \param amplitude: Amplitude
/// \param T : The period
/// \param phi : the phase
/// \param T_min : lower bound of the time interval (default to 0)
/// \param T_max : upper bound of the time interval (default to +inf)
///
sinusoidal(const Point& p0, const Point& amplitude, const time_t T, const time_t phi, const time_t T_min = 0.,
const time_t T_max = std::numeric_limits<time_t>::max())
: p0_(p0),
amplitude_(amplitude),
T_(T),
phi_(std::fmod(phi, 2. * M_PI)),
T_min_(T_min),
T_max_(T_max),
dim_(p0_.size()) {
if (Safe && T_min_ > T_max_) {
throw std::invalid_argument("can't create constant curve: min bound is higher than max bound");
}
if (T_ <= 0.) throw std::invalid_argument("The period must be strictly positive");
if (static_cast<size_t>(amplitude_.size()) != dim_)
throw std::invalid_argument("The offset and the amplitude must have the same dimension");
}
/// \brief Constructor from stationary points
/// \param traj_time: duration to go from p_init to p_final (half a period)
/// \param p_init : first stationary point, either minimum or maximum
/// \param p_final : second stationary point, either minimum or maximum
/// \param T_min : lower bound of the time interval (default to 0)
/// \param T_max : upper bound of the time interval (default to +inf)
///
sinusoidal(const time_t traj_time, const Point& p_init, const Point& p_final, const time_t T_min = 0.,
const time_t T_max = std::numeric_limits<time_t>::max())
: T_(2. * traj_time), phi_(M_PI / 2.), T_min_(T_min), T_max_(T_max), dim_(p_init.size()) {
if (Safe && T_min_ > T_max_) {
throw std::invalid_argument("can't create constant curve: min bound is higher than max bound");
}
if (T_ <= 0) throw std::invalid_argument("The period must be strictly positive");
if (p_init.size() != p_final.size())
throw std::invalid_argument("The two stationary points must have the same dimension");
p0_ = (p_init + p_final) / 2.;
amplitude_ = (p_init - p_final) / 2.;
}
/// \brief Copy constructor
/// \param other
sinusoidal(const sinusoidal_t& other)
: p0_(other.p0_),
amplitude_(other.amplitude_),
T_(other.T_),
phi_(other.phi_),
T_min_(other.T_min_),
T_max_(other.T_max_),
dim_(other.dim_) {}
/// \brief Destructor.
virtual ~sinusoidal() {}
/* 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 sinusoidal curve : time t to evaluate should be in range [Tmin, Tmax] of the curve");
}
return p0_ + amplitude_ * sin(two_pi_f(t) + phi_);
}
/// \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 order) 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");
}
if (order <= 0) throw std::invalid_argument("Order must be strictly positive");
return amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order)) *
sin(two_pi_f(t) + phi_ + (M_PI * static_cast<num_t>(order) / 2.));
}
/// \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.
sinusoidal_t compute_derivate(const std::size_t order) const {
if (order <= 0) throw std::invalid_argument("Order must be strictly positive");
const point_t amplitude = amplitude_ * pow(2. * M_PI / T_, static_cast<num_t>(order));
const time_t phi = phi_ + (M_PI * static_cast<num_t>(order) / 2.);
return sinusoidal_t(point_t::Zero(dim_), amplitude, T_, phi, T_min_, T_max_);
}
/// \brief Compute the derived curve at orderN.
/// \param order : order of derivative.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
virtual sinusoidal_t* compute_derivate_ptr(const std::size_t order) const {
return new sinusoidal_t(compute_derivate(order));
}
/**
* @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 sinusoidal_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return curves::isApprox<time_t>(T_min_, other.min()) && curves::isApprox<time_t>(T_max_, other.max()) &&
dim_ == other.dim() && p0_.isApprox(other.p0_, prec) && amplitude_.isApprox(other.amplitude_, prec) &&
curves::isApprox<time_t>(T_, other.T_) && curves::isApprox<time_t>(phi_, other.phi_);
}
virtual bool isApprox(const curve_abc_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
const sinusoidal_t* other_cast = dynamic_cast<const sinusoidal_t*>(other);
if (other_cast)
return isApprox(*other_cast, prec);
else
return false;
}
virtual bool operator==(const sinusoidal_t& other) const { return isApprox(other); }
virtual bool operator!=(const sinusoidal_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 1; }
/*Helpers*/
/*Attributes*/
Point p0_; // offset
Point amplitude_;
time_t T_; // period
time_t phi_; // phase
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("p0", p0_);
ar& boost::serialization::make_nvp("amplitude_", amplitude_);
ar& boost::serialization::make_nvp("T_", T_);
ar& boost::serialization::make_nvp("phi_", phi_);
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_);
}
private:
inline const num_t two_pi_f(const time_t& t) const { return (2 * M_PI / T_) * t; }
}; // struct sinusoidal
} // namespace curves
#endif // _CLASS_SINUSOIDALCURVE
......@@ -4,6 +4,7 @@
#include "MathDefs.h"
#include "curve_abc.h"
#include "constant_curve.h"
#include <Eigen/Geometry>
#include <boost/math/constants/constants.hpp>
......@@ -24,8 +25,10 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
typedef Eigen::Quaternion<Scalar> quaternion_t;
typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;
typedef constant_curve<Time, Numeric, Safe, point_derivate_t> curve_derivate_t;
typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
public:
/* Constructors - destructors */
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
......@@ -161,14 +164,14 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
}
}
SO3Linear_t compute_derivate(const std::size_t /*order*/) const {
throw std::logic_error("Compute derivate for SO3Linear is not implemented yet.");
curve_derivate_t compute_derivate(const std::size_t order) const {
return curve_derivate_t(derivate(T_min_, order), 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.
SO3Linear_t* compute_derivate_ptr(const std::size_t order) const { return new SO3Linear_t(compute_derivate(order)); }
curve_derivate_t* compute_derivate_ptr(const std::size_t order) const { return new curve_derivate_t(compute_derivate(order)); }
/*Helpers*/
/// \brief Get dimension of curve.
......
......@@ -5,5 +5,8 @@ PYTHON_INSTALL_ON_SITE(${PROJECT_NAME} plot.py)
PYTHON_INSTALL_ON_SITE(${PROJECT_NAME} optimization.py)
ADD_PYTHON_UNIT_TEST("python-curves" "python/test/test.py" "python")
ADD_PYTHON_UNIT_TEST("python-constant" "python/test/test-constant.py" "python")
ADD_PYTHON_UNIT_TEST("python-sinusoidal" "python/test/test-sinusoidal.py" "python")
ADD_PYTHON_UNIT_TEST("python-minjerk" "python/test/test-minjerk.py" "python")
ADD_PYTHON_UNIT_TEST("python-optimization" "python/test/optimization.py" "python")
ADD_PYTHON_UNIT_TEST("python-notebook" "python/test/notebook.py" "python")
This diff is collapsed.
#include "curves/fwd.h"
#include "curves/linear_variable.h"
#include "curves/bezier_curve.h"
#include "curves/constant_curve.h"
#include "curves/polynomial.h"
#include "curves/exact_cubic.h"
#include "curves/curve_constraint.h"
......@@ -10,6 +11,7 @@
#include "curves/piecewise_curve.h"
#include "curves/so3_linear.h"
#include "curves/se3_curve.h"
#include "curves/sinusoidal.h"
#include "curves/python/python_definitions.h"
#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>
......@@ -71,15 +73,18 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::matrix_pair)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::constant_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::polynomial3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::exact_cubic3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::constant3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::cubic_hermite_spline3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::SO3Linear_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::SE3Curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::sinusoidal_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_SE3_t)
#endif //_VARIABLES_PYTHON_BINDINGS
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import unittest
import curves
from curves import constant, constant3
import numpy as np
from numpy import array, isclose, array_equal
class ConstantCurveTest(unittest.TestCase):
def test_constructor(self):
# default constructor
c = constant()
self.assertEqual(c.min(), 0.)
self.assertEqual(c.max(), 0)
self.assertEqual(c.dim(), 0)
self.assertEqual(c.degree(), 0)
# constructor from a point
p = array([1, 23., 5., 9, -5])
c = constant(p)
self.assertEqual(c.min(), 0.)
self.assertTrue(c.max() > 1e100) # convert std::numeric_limits<time_t>::max() to python ?
self.assertEqual(c.dim(), 5)
self.assertEqual(c.degree(), 0)
# constructor with timing
c = constant(p, 1., 5.)
self.assertEqual(c.min(), 1.)
self.assertEqual(c.max(), 5.)
self.assertEqual(c.dim(), 5)
self.assertEqual(c.degree(), 0)
with self.assertRaises(ValueError):
c = constant(p, 2., 1.)
def test_evaluate(self):
p = array([1, 23., 5., 9, -5])
c = constant(p, 1., 3.)
self.assertTrue(array_equal(c(1.), p))
self.assertTrue(array_equal(c(2.5), p))
self.assertTrue(array_equal(c(3.), p))
with self.assertRaises(ValueError):
c(0.5)
with self.assertRaises(ValueError):
c(4.)
def test_derivate(self):
p = array([1, 23., 5., 9, -5])
p0 = np.zeros(5)
c = constant(p, 1., 3.)
self.assertTrue(array_equal(c.derivate(1., 1), p0))
self.assertTrue(array_equal(c.derivate(1., 2), p0))
self.assertTrue(array_equal(c.derivate(3., 1), p0))
self.assertTrue(array_equal(c.derivate(1.5, 1), p0))
with self.assertRaises(ValueError):
c.derivate(0.5, 1)
with self.assertRaises(ValueError):
c.derivate(4., 3)
c_deriv = c.compute_derivate(1)
self.assertTrue(array_equal(c_deriv(1.5), p0))
self.assertEqual(c_deriv.min(), c.min())
self.assertEqual(c_deriv.max(), c.max())
self.assertEqual(c_deriv.dim(), c.dim())
def test_comparator(self):
p = array([1, 23., 5., 9, -5])
c1 = constant(p, 1., 3.)
c2 = constant(p, 1., 3.)
c3 = c1
pn = array([1, 23., 5., 9])
p2 = array([1, 17., 5., 9, -5])
cn1 = constant(pn, 1., 3.)
cn2 = constant(p, 1.5, 3.)
cn3 = constant(p, 1., 2.)
cn4 = constant(p2, 1., 3.)
self.assertEqual(c1, c2)
self.assertEqual(c1, c3)
self.assertNotEqual(c1, cn1)
self.assertNotEqual(c1, cn2)
self.assertNotEqual(c1, cn3)
self.assertNotEqual(c1, cn4)
self.assertTrue(c1.isEquivalent(c2))
def test_serialization(self):
p = array([1, 23., 5., 9, -5])
c = constant(p, 1., 3.)
c.saveAsText("serialization_curve.txt")
c.saveAsXML("serialization_curve.xml", "constant")
c.saveAsBinary("serialization_curve")
c_txt = constant()
c_txt.loadFromText("serialization_curve.txt")
self.assertEqual(c, c_txt)
c_xml = constant()
c_xml.loadFromXML("serialization_curve.xml", "constant")
self.assertEqual(c, c_xml)
c_bin = constant()
c_bin.loadFromBinary("serialization_curve")
self.assertEqual(c, c_bin)
class Constant3CurveTest(unittest.TestCase):
def test_constructor(self):
# default constructor
c = constant()
self.assertEqual(c.min(), 0.)
self.assertEqual(c.max(), 0)
self.assertEqual(c.dim(), 0)
self.assertEqual(c.degree(), 0)
# constructor from a point
p = array([1, 23., 5.])
c = constant3(p)
self.assertEqual(c.min(), 0.)
self.assertTrue(c.max() > 1e100) # convert std::numeric_limits<time_t>::max() to python ?
self.assertEqual(c.dim(), 3)
self.assertEqual(c.degree(), 0)
# constructor with timing
c = constant3(p, 1., 5.)
self.assertEqual(c.min(), 1.)
self.assertEqual(c.max(), 5.)
self.assertEqual(c.dim(), 3)
self.assertEqual(c.degree(), 0)
with self.assertRaises(ValueError):
c = constant(p, 2., 1.)
def test_serialization(self):
p = array([1, 23., 5.])
c = constant3(p, 0., 2.)
c.saveAsText("serialization_constant.txt")
c.saveAsXML("serialization_constant.xml", "constant")
c.saveAsBinary("serialization_constant")
c_txt = constant3()
c_txt.loadFromText("serialization_constant.txt")
self.assertEqual(c, c_txt)
c_xml = constant3()
c_xml.loadFromXML("serialization_constant.xml", "constant")
self.assertEqual(c, c_xml)
c_bin = constant3()
c_bin.loadFromBinary("serialization_constant")
self.assertEqual(c, c_bin)
if __name__ == '__main__':
unittest.main()
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import unittest
import curves
from curves import polynomial
import numpy as np
from numpy import array, isclose, array_equal
class MinJerkCurveTest(unittest.TestCase):
def test_constructors(self):
# constructor from two points
init = array([1, 23., 5., 9, -5])
end = array([1.2, -5.1, 5.6, 1, -2])
c = polynomial.MinimumJerk(init, end)
self.assertEqual(c.min(), 0.)
self.assertEqual(c.max(), 1.)
self.assertEqual(c.dim(), 5)
self.assertEqual(c.degree(), 5)
self.assertTrue(isclose(c(0.), init).all())
self.assertTrue(isclose(c(1.), end).all())
# constructor with timing
c = polynomial.MinimumJerk(init, end, 2., 6.)
self.assertEqual(c.min(), 2.)
self.assertEqual(c.max(), 6.)
self.assertEqual(c.dim(), 5)
self.assertEqual(c.degree(), 5)
self.assertTrue(isclose(c(2.), init).all())
self.assertTrue(isclose(c(6.), end).all())
end4 = array([1.2, -5.1, 5.6, 1])
with self.assertRaises(ValueError):
polynomial.MinimumJerk(init, end4)
with self.assertRaises(ValueError):
polynomial.MinimumJerk(init, end, 2., 0.)
def test_derivate(self):
init = array([1, 23., 5., 9, -5])
end = array([1.2, -5.1, 5.6, 1, -2])
p0 = np.zeros(5)
c = polynomial.MinimumJerk(init, end, 2., 6.)
self.assertTrue(isclose(c.derivate(2., 1), p0).all())
self.assertTrue(isclose(c.derivate(2., 2), p0).all())
self.assertTrue(isclose(c.derivate(6., 1), p0).all())
self.assertTrue(isclose(c.derivate(6., 2), p0).all())
self.assertTrue(isclose(c.derivate(3., 6), p0).all())
if __name__ == '__main__':
unittest.main()
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import unittest
import curves
from curves import sinusoidal
import numpy as np
from numpy import array, isclose, array_equal
class SinusoidalCurveTest(unittest.TestCase):
def test_constructor(self):
# default constructor
c = sinusoidal()
self.assertEqual(c.min(), 0.)
self.assertEqual(c.max(), 0)
self.assertEqual(c.dim(), 0)
self.assertEqual(c.degree(), 1)
# constructor without time
p0 = array([1, -23., 5.])
amp = array([-1, 0.6, 2.8])
T = 1.5
phi = 0.2
c = sinusoidal(p0, amp, T, phi)
self.assertEqual(c.min(), 0.)
self.assertTrue(c.max() > 1e100) # convert std::numeric_limits<time_t>::max() to python ?
self.assertEqual(c.dim(), 3)
self.assertEqual(c.degree(), 1)
# constructor with timing
c = sinusoidal(p0, amp, T, phi, 1., 5.)
self.assertEqual(c.min(), 1.)
self.assertEqual(c.max(), 5.)
self.assertEqual(c.dim(), 3)
self.assertEqual(c.degree(), 1)
# constructor from stationary points
p_init = array([1.2, -5, 3.6, 5.])
p_end = array([-1.2, -3, 2.7, -3.2])
c = sinusoidal(1.5, p_init, p_end)
self.assertEqual(c.min(), 0.)
self.assertTrue(c.max() > 1e100) # convert std::numeric_limits<time_t>::max() to python ?
self.assertEqual(c.dim(), 4)
self.assertEqual(c.degree(), 1)
# constructor from stationary points with timing
p_init = array([1.2, -5, 3.6, 5.])
p_end = array([-1.2, -3, 2.7, -3.2])
c = sinusoidal(1.5, p_init, p_end, 1.5, 25)
self.assertEqual(c.min(), 1.5)
self.assertEqual(c.max(), 25.) # convert std::numeric_limits<time_t>::max() to python ?
self.assertEqual(c.dim(), 4)
self.assertEqual(c.degree(), 1)
with self.assertRaises(ValueError):
c = sinusoidal(p0, amp, T, phi, 2., 1.)
def test_evaluate(self):
p0 = array([1, -23., 5.])
amp = array([-1, 0.6, 2.8])
T = 1.5
phi = 0.
c = sinusoidal(p0, amp, T, phi, 0., 20.)
self.assertTrue(isclose(c(0), p0).all())
self.assertTrue(isclose(c(T), p0).all())
self.assertTrue(isclose(c(T / 2.), p0).all())
self.assertTrue(isclose(c(9. * T), p0).all())
self.assertTrue(isclose(c(T / 4.), p0 + amp).all())
self.assertTrue(isclose(c(3. * T / 4.), p0 - amp).all())
with self.assertRaises(ValueError):
c(-0.5)
with self.assertRaises(ValueError):
c(24.)
def test_derivate(self):
p0 = array([1, -23., 5.])
amp = array([-1, 0.6, 2.8])
T = 1.5
phi = 0.
c = sinusoidal(p0, amp, T, phi, 0., 20.)
self.assertTrue(isclose(c.derivate(0., 1), (amp * 2. * np.pi / T)).all())
self.assertTrue(isclose(c.derivate(T, 1), (amp * 2. * np.pi / T)).all())
self.assertTrue(isclose(c.derivate(T / 2., 1), (-amp * 2. * np.pi / T)).all())
self.assertTrue(isclose(c.derivate(T / 4., 1), np.zeros(3)).all())
self.assertTrue(isclose(c.derivate(3. * T / 4., 1), np.zeros(3)).all())
self.assertTrue(isclose(c.derivate(T / 4., 2), (-(amp * 2. * np.pi / T) * (2. * np.pi / T))).all())
self.assertTrue(isclose(c.derivate(3. * T / 4., 2), ((amp * 2. * np.pi / T) * (2. * np.pi / T))).all())
self.assertTrue(isclose(c.derivate(0, 2), np.zeros(3)).all())
self.assertTrue(isclose(c.derivate(T, 2), np.zeros(3)).all())
self.assertTrue(isclose(c.derivate(2. * T, 2), np.zeros(3)).all())
with self.assertRaises(ValueError):
c.derivate(-0.5, 1)
with self.assertRaises(ValueError):
c.derivate(54., 3)
with self.assertRaises(OverflowError):
c.derivate(1., -1)
with self.assertRaises(ValueError):
c.derivate(1., 0)
for i in range(1, 10):
c_deriv = c.compute_derivate(i)
self.assertEqual(c_deriv.min(), c.min())
self.assertEqual(c_deriv.max(), c.max())
self.assertEqual(c_deriv.dim(), c.dim())
self.assertTrue(isclose(c_deriv(0.), c.derivate(0., i)).all())
self.assertTrue(isclose(c_deriv(0.2), c.derivate(0.2, i)).all())
self.assertTrue(isclose(c_deriv(0.6), c.derivate(0.6, i)).all())
self.assertTrue(isclose(c_deriv(0.9), c.derivate(0.9, i)).all())
self.assertTrue(isclose(c_deriv(T), c.derivate(T, i)).all())
self.assertTrue(isclose(c_deriv(T * 2.), c.derivate(T * 2., i)).all())
def test_comparator(self):
p0 = array([1, -23., 5.])
amp = array([-1, 0.6, 2.8])
T = 1.5
phi = 0.
c01 = sinusoidal(p0, amp, T, phi)
c02 = sinusoidal(p0, amp, T, phi)
self.assertEqual(c01, c02)
c1 = sinusoidal(p0, amp, T, phi, 1., 10.)
c2 = sinusoidal(p0, amp, T, phi, 1., 10.)
c3 = c1
self.assertEqual(c1, c2)
self.assertEqual(c1, c3)
self.assertTrue(c1.isEquivalent(c2))
p01 = array([1.5, -23., 5.])
amp1 = array([-1, -0.6, 2.8])
cn1 = sinusoidal(p01, amp, T, phi, 1., 10.)
cn2 = sinusoidal(p0, amp1, 1.4, phi, 1., 10.)
cn3 = sinusoidal(p0, amp, T, 0.2, 1., 10.)
cn4 = sinusoidal(p0, amp, T, phi, 0., 10.)
cn5 = sinusoidal(p0, amp, T, phi, 1., 5.)
self.assertNotEqual(c1, cn1)
self.assertNotEqual(c1, cn2)
self.assertNotEqual(c1, cn3)
self.assertNotEqual(c1, cn4)
self.assertNotEqual(c1, cn5)
self.assertTrue(c1.isEquivalent(c2))
def test_serialization(self):
p0 = array([1, -23., 5.])
amp = array([-1, 0.6, 2.8])
T = 1.5
phi = 0.2
c = sinusoidal(p0, amp, T, phi, 2., 5.)
c.saveAsText("serialization_sinusoidal.txt")
c.saveAsXML("serialization_sinusoidal.xml", "sinusoidal")
c.saveAsBinary("serialization_sinusoidal")
c_txt = sinusoidal()
c_txt.loadFromText("serialization_sinusoidal.txt")
self.assertEqual(c, c_txt)
c_xml = sinusoidal()
c_xml.loadFromXML("serialization_sinusoidal.xml", "sinusoidal")
self.assertEqual(c, c_xml)
c_bin = sinusoidal()
c_bin.loadFromBinary("serialization_sinusoidal")
self.assertEqual(c, c_bin)
if __name__ == '__main__':
unittest.main()
ADD_UNIT_TEST(curves_tests Main.cpp)
TARGET_LINK_LIBRARIES(curves_tests ${PROJECT_NAME} ${Boost_LIBRARIES})
TARGET_COMPILE_DEFINITIONS(curves_tests PRIVATE -DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/data/")
SET(${PROJECT_NAME}_TESTS
test-constant
test-sinusoidal
test-minjerk
)
FOREACH(TEST ${${PROJECT_NAME}_TESTS})
ADD_UNIT_TEST(${TEST} ${TEST})
TARGET_LINK_LIBRARIES(${TEST} ${PROJECT_NAME} ${Boost_LIBRARIES})
ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS})
......@@ -11,6 +11,7 @@
#include "load_problem.h"
#include "curves/so3_linear.h"
#include "curves/se3_curve.h"
#include "curves/serialization/curves.hpp"
#include <string>
#include <iostream>
#include <cmath>
......@@ -1447,6 +1448,113 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
}
}
void PiecewisePolynomialCurveFromFile(bool& error){
std::string filename_pos(TEST_DATA_PATH "discrete_points_pos.txt");
std::string filename_vel(TEST_DATA_PATH "discrete_points_vel.txt");
std::string filename_acc(TEST_DATA_PATH "discrete_points_acc.txt");
std::string filename_error(TEST_DATA_PATH "discrete_points_error.txt");
piecewise_t c_pos = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_pos, 0.01, 3);
if(c_pos.min() != 0.){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
error = true;
}
if(c_pos.max() != 0.03){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.03" << std::endl;
error = true;
}
pointX_t p0(3), p2(3);
p0<<-0.003860389372941039, 0.0012353625242474164, 0.009005041639999767;
p2<<-0.0028803627898293283, 0.0011918668401150736, 0.009005041639999767;
if(! c_pos(0.).isApprox(p0)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_pos(0.02).isApprox(p2)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
piecewise_t c_vel = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_vel, 0.05, 3);
if(c_pos.min() != 0.){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
error = true;
}
if(! QuasiEqual(c_vel.max(), 0.15)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.15" << std::endl;
error = true;
}
pointX_t p3(3);
p3<<0.2968141884672718, 0.0012916907964522569, 0.00951023474821927;
if(! c_vel(0.).isApprox(p0)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_vel(0.15).isApprox(p3)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_vel.derivate(0., 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at 0. should be null" << std::endl;
error = true;
}
if(! c_vel.derivate(0.1, 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at 0.1 should be null" << std::endl;
error = true;
}
piecewise_t c_acc = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc, 0.001, 3);
if(c_acc.min() != 0.){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
error = true;
}
if(! QuasiEqual(c_acc.max(), 7.85)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 7.85" << std::endl;
error = true;
}
if(! c_acc(0.).isApprox(p0)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
pointX_t p5200(3);
p5200<<0.30273356072723845, -0.07619420199174821, 0.010015348526727433;
if(! c_acc(5.2).isApprox(p5200)){
std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
error = true;
}
if(! c_acc.derivate(0., 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative at 0 should be null" << std::endl;
error = true;
}
if(! c_acc.derivate(0.5, 1).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative should at 0.5 be null" << std::endl;
error = true;
}
if(! c_acc.derivate(0., 2).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second derivative at 0 should be null" << std::endl;
error = true;
}
if(! c_acc.derivate(5., 2).isZero()){
std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second derivative at 5 should be null" << std::endl;
error = true;
}
try {
piecewise_t c_error = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc, 0.01, 4);
std::cout << "PiecewisePolynomialCurveFromFile, Error, dimension do not match, an error should be raised" << std::endl;
error = true;
} catch (std::invalid_argument e) {
}
try {
piecewise_t c_error = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_error, 0.01, 3);
std::cout << "PiecewisePolynomialCurveFromFile, Error, discrete_points_error should not be parsed correctly" << std::endl;
error = true;
} catch (std::invalid_argument e) {
}
}
void serializationCurvesTest(bool& error) {
try {
std::string errMsg1("in serializationCurveTest, Error While serializing Polynomial : ");
......@@ -1725,6 +1833,20 @@ void so3LinearTest(bool& error) {
error = true;
std::cout << "Angular velocity around y and z axis should be null" << std::endl;
}
constant3_t so3Derivate1 = so3Traj.compute_derivate(1);
if (so3Derivate1(1.) != so3Traj.derivate(1., 1)) {
error = true;
std::cout << "compute_derivate curve do not equal derivate call" << std::endl;
}
constant3_t so3Derivate2 = so3Traj.compute_derivate(2);
if (so3Derivate2(1.) != point3_t::Zero(3)) {
error = true;
std::cout << "compute_derivate curve do not equal derivate call" << std::endl;
}
// check if errors are correctly raised :
try {
so3Traj(-0.1);
......@@ -2754,6 +2876,7 @@ int main(int /*argc*/, char** /*argv[]*/) {
CubicHermitePairsPositionDerivativeTest(error);
piecewiseCurveTest(error);
PiecewisePolynomialCurveFromDiscretePoints(error);
PiecewisePolynomialCurveFromFile(error);
toPolynomialConversionTest(error);
cubicConversionTest(error);
curveAbcDimDynamicTest(error);
......
This diff is collapsed.
-0.003860389372941039 0.0012353625242474164 0.009005041639999767
-0.003367975609860228 0.0012135142149832263 0.009005041639999767
-0.0028803627898293283 0.0011918668401150736
-0.0023975258947521072 0.0011704190312931094 0.009005041639999767
-0.003860389372941039 0.0012353625242474164 0.009005041639999767
-0.003367975609860228 0.0012135142149832263 0.009005041639999767
-0.0028803627898293283 0.0011918668401150736 0.009005041639999767
-0.0023975258947521072 0.0011704190312931094 0.009005041639999767
-0.003860389372941039 0.0012353625242474164 0.009005041639999767 0 0 0
-0.003367975609860228 0.0012135142149832263 0.009005041639999767 0 0 0
0.2968095500861058 0.0013266763658670213 0.00951023474821927 0 0 0