Commit 88ffc79b authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format]

parent 0e28b340
Pipeline #11481 failed with stage
in 4 minutes and 5 seconds
/**
* \file Math.h
* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains math definitions used
* used throughout the library.
* Preprocessors definition are used to use eitheir float
* or double values, and 3 dimensional vectors for
* the Point structure.
*/
* \file Math.h
* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains math definitions used
* used throughout the library.
* Preprocessors definition are used to use eitheir float
* or double values, and 3 dimensional vectors for
* the Point structure.
*/
#ifndef _SPLINEMATH
#define _SPLINEMATH
......@@ -20,23 +20,20 @@
#include <vector>
#include <utility>
namespace curves{
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler= 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
{
m_sigma_inv(i,i)=1.0/m_sigma(i);
}
namespace curves {
// REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template <typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat) {
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler = 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
for (long i = 0; i < m_sigma.rows(); ++i) {
if (m_sigma(i) > pinvtoler) {
m_sigma_inv(i, i) = 1.0 / m_sigma(i);
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
} // namespace curves
#endif //_SPLINEMATH
pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
}
} // namespace curves
#endif //_SPLINEMATH
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN
......@@ -18,72 +17,62 @@
#include <vector>
#include <stdexcept>
namespace curves
{
/// \brief Computes a binomial coefficient .
/// \param n : an unsigned integer.
/// \param k : an unsigned integer.
/// \return \f$\binom{n}{k}f$
///
inline unsigned int bin(const unsigned int n, const unsigned int k)
{
if(k > n) throw std::runtime_error("binomial coefficient higher than degree");
if(k == 0) return 1;
if(k > n/2) return bin(n,n-k);
return n * bin(n-1,k-1) / k;
}
namespace curves {
/// \brief Computes a binomial coefficient .
/// \param n : an unsigned integer.
/// \param k : an unsigned integer.
/// \return \f$\binom{n}{k}f$
///
inline unsigned int bin(const unsigned int n, const unsigned int k) {
if (k > n) throw std::runtime_error("binomial coefficient higher than degree");
if (k == 0) return 1;
if (k > n / 2) return bin(n, n - k);
return n * bin(n - 1, k - 1) / k;
}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template <typename Numeric = double>
struct Bern {
Bern(){}
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i),
i_(i),
bin_m_i_(bin(m,i))
{}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template <typename Numeric = double>
struct Bern {
Bern() {}
Bern(const unsigned int m, const unsigned int i) : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
~Bern(){}
~Bern() {}
Numeric operator()(const Numeric u) const
{
assert(u >= 0. && u <= 1.);
return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i);
}
Numeric operator()(const Numeric u) const {
assert(u >= 0. && u <= 1.);
return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
}
/* Attributes */
Numeric m_minus_i;
Numeric i_;
Numeric bin_m_i_;
/* Attributes */
/* Attributes */
Numeric m_minus_i;
Numeric i_;
Numeric bin_m_i_;
/* Attributes */
// Serialization of the class
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){
if (version) {
// 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::make_nvp("m_minus_i", m_minus_i);
ar & boost::serialization::make_nvp("i", i_);
ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
}
}; // End struct Bern
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template <typename Numeric>
std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
{
std::vector<Bern<Numeric> > res;
for(unsigned int i = 0; i<= n; ++i)
{
res.push_back(Bern<Numeric>(n, i));
}
return res;
ar& boost::serialization::make_nvp("m_minus_i", m_minus_i);
ar& boost::serialization::make_nvp("i", i_);
ar& boost::serialization::make_nvp("bin_m_i", bin_m_i_);
}
} // namespace curves
#endif //_CLASS_BERNSTEIN
}; // End struct Bern
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template <typename Numeric>
std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) {
std::vector<Bern<Numeric> > res;
for (unsigned int i = 0; i <= n; ++i) {
res.push_back(Bern<Numeric>(n, i));
}
return res;
}
} // namespace curves
#endif //_CLASS_BERNSTEIN
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE
......@@ -21,469 +20,422 @@
#include <iostream>
namespace curves
{
/// \class BezierCurve.
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
namespace curves {
/// \class BezierCurve.
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
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 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;
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
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 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;
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
bezier_curve()
: dim_(0), T_min_(0), T_max_(0)
{}
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
/// \param mult_T : ... (default value is 1.0).
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
: T_min_(T_min),
T_max_(T_max),
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd)),
degree_(size_-1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
control_points_.push_back(*it);
}
// set dim
if (control_points_.size()!=0)
{
dim_ = PointsBegin->size();
}
}
/// \brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param constraints : constraints applying on start / end velocities and acceleration.
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints,
const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
: T_min_(T_min),
T_max_(T_max),
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd)+4),
degree_(size_-1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
{
control_points_.push_back(*cit);
}
// set dim
if (control_points_.size()!=0)
{
dim_ = PointsBegin->size();
}
}
bezier_curve(const bezier_curve& other)
: dim_(other.dim_), T_min_(other.T_min_), T_max_(other.T_max_),
mult_T_(other.mult_T_), size_(other.size_),
degree_(other.degree_), bernstein_(other.bernstein_),
control_points_(other.control_points_)
{}
///\brief Destructor
~bezier_curve()
{
// NOTHING
}
/*Operations*/
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
check_conditions();
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
}
if (size_ == 1)
{
return mult_T_*control_points_[0];
}
else
{
return evalHorner(t);
}
}
/// \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.
bezier_curve_t compute_derivate(const std::size_t order) const
{
check_conditions();
if(order == 0)
{
return *this;
}
t_point_t derived_wp;
for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end()-1; ++pit)
{
derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
}
if(derived_wp.empty())
{
derived_wp.push_back(point_t::Zero(dim_));
}
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
return deriv.compute_derivate(order-1);
}
/// \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$.
/// \param order : order of the primitive.
/// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
check_conditions();
if(order == 0)
{
return *this;
}
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
point_t current_sum = point_t::Zero(dim_);
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
// then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
n_wp.push_back(current_sum);
for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit)
{
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_));
return integ.compute_primitive(order-1);
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
/// \param order : order of derivative.
/// \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);
}
/// \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$
/// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
/// Warning: the horner scheme is about 100 times faster than this method.<br>
/// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalBernstein(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
point_t res = point_t::Zero(dim_);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++control_points_it)
{
res += cit->operator()(u) * (*control_points_it);
}
return res*mult_T_;
}
/// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
/// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
/// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
/// Horner's scheme : for a polynom of degree N expressed by : <br>
/// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
/// Using Horner's method, the polynom is transformed into : <br>
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalHorner(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
Numeric u_op, bc, tn;
u_op = 1.0 - u;
bc = 1;
tn = 1;
point_t tmp =(*control_points_it)*u_op; ++control_points_it;
for(unsigned int i=1; i<degree_; i++, ++control_points_it)
{
tn = tn*u;
bc = bc*((num_t)(degree_-i+1))/i;
tmp = (tmp + tn*bc*(*control_points_it))*u_op;
}
return (tmp + tn*u*(*control_points_it))*mult_T_;
}
const t_point_t& waypoints() const {return control_points_;}
const point_t waypointAtIndex(const std::size_t index) const
{
point_t waypoint;
if (index<=control_points_.size())
{
waypoint = control_points_[index];
}
return waypoint;
}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
/// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
/// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
/// curve at time \f$t\f$.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalDeCasteljau(const Numeric t) const
{
// normalize time :
const Numeric u = (t-T_min_)/(T_max_-T_min_);
t_point_t pts = deCasteljauReduction(waypoints(),u);
while(pts.size() > 1)
{
pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
}
t_point_t deCasteljauReduction(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
return deCasteljauReduction(waypoints(),u);
}
/// \brief Compute de Casteljau's reduction of the given list of points at time t.
/// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
/// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
/// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
/// parameters \f${t,1-t}\f$ of consecutive points in the list.
/// \param pts : list of points.
/// \param u : NORMALIZED time when to evaluate the curve.
/// \return reduced list of point (size of pts - 1).
///
t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const
{
if(u < 0 || u > 1)
{
throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
}
if(pts.size() == 1)
{
return pts;
}
t_point_t new_pts;
for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
{
new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
}
return new_pts;
}
/// \brief Split the bezier curve in 2 at time t.
/// \param t : list of points.
/// \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)
{
check_conditions();
if (fabs(t-T_max_)<MARGIN)
{
throw std::runtime_error("can't split curve, interval range is equal to original curve");
}
t_point_t wps_first(size_),wps_second(size_);
const Numeric u = (t-T_min_)/(T_max_-T_min_);
t_point_t casteljau_pts = waypoints();
wps_first[0] = casteljau_pts.front();
wps_second[degree_] = casteljau_pts.back();
size_t id = 1;
while(casteljau_pts.size() > 1)
{
casteljau_pts = deCasteljauReduction(casteljau_pts,u);
wps_first[id] = casteljau_pts.front();
wps_second[degree_-id] = casteljau_pts.back();
++id;
}
bezier_curve_t c_first(wps_first.begin(), wps_first.end(),T_min_,t,mult_T_);
bezier_curve_t c_second(wps_second.begin(), wps_second.end(),t, T_max_,mult_T_);
return std::make_pair(c_first,c_second);
}
/// \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.
/// \param t2 : end time of bezier curve extracted.
/// \return bezier curve extract defined between \f$[t_1,t_2]\f$.
///
bezier_curve_t extract(const Numeric t1, const Numeric t2)
{
if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
{
throw std::out_of_range("In Extract curve : times out of bounds");
}
if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN) // t1=T_min and t2=T_max
{
return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
}
if (fabs(t1-T_min_)<MARGIN) // t1=T_min
{