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 * \file Math.h
* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 06/17/2013 * \date 06/17/2013
* *
* This file contains math definitions used * This file contains math definitions used
* used throughout the library. * used throughout the library.
* Preprocessors definition are used to use eitheir float * Preprocessors definition are used to use eitheir float
* or double values, and 3 dimensional vectors for * or double values, and 3 dimensional vectors for
* the Point structure. * the Point structure.
*/ */
#ifndef _SPLINEMATH #ifndef _SPLINEMATH
#define _SPLINEMATH #define _SPLINEMATH
...@@ -20,23 +20,20 @@ ...@@ -20,23 +20,20 @@
#include <vector> #include <vector>
#include <utility> #include <utility>
namespace curves{ namespace curves {
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels // REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_> template <typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat) void PseudoInverse(_Matrix_Type_& pinvmat) {
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues(); _Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler= 1.e-6; // choose your tolerance widely! double pinvtoler = 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows()); _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i) for (long i = 0; i < m_sigma.rows(); ++i) {
{ if (m_sigma(i) > pinvtoler) {
if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i);
{
m_sigma_inv(i,i)=1.0/m_sigma(i);
} }
} }
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose()); pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
} }
} // namespace curves } // namespace curves
#endif //_SPLINEMATH #endif //_SPLINEMATH
/** /**
* \file bezier_curve.h * \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3. * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 06/17/2013 * \date 06/17/2013
*/ */
#ifndef _CLASS_BERNSTEIN #ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN #define _CLASS_BERNSTEIN
...@@ -18,39 +17,32 @@ ...@@ -18,39 +17,32 @@
#include <vector> #include <vector>
#include <stdexcept> #include <stdexcept>
namespace curves namespace curves {
{ /// \brief Computes a binomial coefficient .
/// \brief Computes a binomial coefficient . /// \param n : an unsigned integer.
/// \param n : an unsigned integer. /// \param k : an unsigned integer.
/// \param k : an unsigned integer. /// \return \f$\binom{n}{k}f$
/// \return \f$\binom{n}{k}f$ ///
/// inline unsigned int bin(const unsigned int n, const unsigned int k) {
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) throw std::runtime_error("binomial coefficient higher than degree"); if (k > n / 2) return bin(n, n - k);
if(k == 0) return 1; return n * bin(n - 1, k - 1) / k;
if(k > n/2) return bin(n,n-k); }
return n * bin(n-1,k-1) / k;
}
/// \class Bernstein. /// \class Bernstein.
/// \brief Computes a Bernstein polynome. /// \brief Computes a Bernstein polynome.
/// ///
template <typename Numeric = double> template <typename Numeric = double>
struct Bern { struct Bern {
Bern(){} Bern() {}
Bern(const unsigned int m, const unsigned int i) Bern(const unsigned int m, const unsigned int i) : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
:m_minus_i(m - i),
i_(i),
bin_m_i_(bin(m,i))
{}
~Bern(){} ~Bern() {}
Numeric operator()(const Numeric u) const Numeric operator()(const Numeric u) const {
{
assert(u >= 0. && u <= 1.); assert(u >= 0. && u <= 1.);
return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i); return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
} }
/* Attributes */ /* Attributes */
...@@ -61,29 +53,26 @@ namespace curves ...@@ -61,29 +53,26 @@ namespace curves
// Serialization of the class // Serialization of the class
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template <class Archive>
void serialize(Archive& ar, const unsigned int version){ void serialize(Archive& ar, const unsigned int version) {
if (version) { if (version) {
// Do something depending on version ? // Do something depending on version ?
} }
ar & boost::serialization::make_nvp("m_minus_i", m_minus_i); ar& boost::serialization::make_nvp("m_minus_i", m_minus_i);
ar & boost::serialization::make_nvp("i", i_); ar& boost::serialization::make_nvp("i", i_);
ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_); ar& boost::serialization::make_nvp("bin_m_i", bin_m_i_);
} }
}; // End struct Bern }; // End struct Bern
/// \brief Computes all Bernstein polynomes for a certain degree. /// \brief Computes all Bernstein polynomes for a certain degree.
/// ///
template <typename Numeric> template <typename Numeric>
std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) {
{
std::vector<Bern<Numeric> > res; std::vector<Bern<Numeric> > res;
for(unsigned int i = 0; i<= n; ++i) for (unsigned int i = 0; i <= n; ++i) {
{
res.push_back(Bern<Numeric>(n, i)); res.push_back(Bern<Numeric>(n, i));
} }
return res; return res;
} }
} // namespace curves } // namespace curves
#endif //_CLASS_BERNSTEIN #endif //_CLASS_BERNSTEIN
This diff is collapsed.
This diff is collapsed.
/** /**
* \file cubic_spline.h * \file cubic_spline.h
* \brief Definition of a cubic spline. * \brief Definition of a cubic spline.
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 06/17/2013 * \date 06/17/2013
* *
* This file contains definitions for the CubicFunction struct. * This file contains definitions for the CubicFunction struct.
* It allows the creation and evaluation of natural * It allows the creation and evaluation of natural
* smooth cubic splines of arbitrary dimension * smooth cubic splines of arbitrary dimension
*/ */
#ifndef _STRUCT_CUBICSPLINE #ifndef _STRUCT_CUBICSPLINE
#define _STRUCT_CUBICSPLINE #define _STRUCT_CUBICSPLINE
...@@ -20,28 +19,27 @@ ...@@ -20,28 +19,27 @@
#include <stdexcept> #include <stdexcept>
namespace curves namespace curves {
{ /// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \brief Creates coefficient vector of a cubic spline defined on the interval /// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br>
/// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br> /// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$ /// with a, b, c and d the control points.
/// with a, b, c and d the control points. ///
/// template <typename Point, typename T_Point>
template<typename Point, typename T_Point> T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const& d) {
T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
{
T_Point res; T_Point res;
res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d); res.push_back(a);
res.push_back(b);
res.push_back(c);
res.push_back(d);
return res; return res;
} }
template<typename Time, typename Numeric, bool Safe, typename Point, typename T_Point> template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point>
polynomial<Time,Numeric,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d, polynomial<Time, Numeric, Safe, Point, T_Point> create_cubic(Point const& a, Point const& b, Point const& c,
const Time t_min, const Time t_max) Point const& d, const Time t_min, const Time t_max) {
{ T_Point coeffs = make_cubic_vector<Point, T_Point>(a, b, c, d);
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d); return polynomial<Time, Numeric, Safe, Point, T_Point>(coeffs.begin(), coeffs.end(), t_min, t_max);
return polynomial<Time,Numeric,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max); }
}
} // namespace curves } // namespace curves
#endif //_STRUCT_CUBICSPLINE #endif //_STRUCT_CUBICSPLINE
/** /**
* \file curve_abc.h * \file curve_abc.h
* \brief interface for a Curve of arbitrary dimension. * \brief interface for a Curve of arbitrary dimension.
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 06/17/2013 * \date 06/17/2013
* *
* Interface for a curve * Interface for a curve
*/ */
#ifndef _STRUCT_CURVE_ABC #ifndef _STRUCT_CURVE_ABC
#define _STRUCT_CURVE_ABC #define _STRUCT_CURVE_ABC
...@@ -18,26 +17,23 @@ ...@@ -18,26 +17,23 @@
#include <functional> #include <functional>
namespace curves namespace curves {
{ /// \struct curve_abc.
/// \struct curve_abc. /// \brief Represents a curve of dimension Dim.
/// \brief Represents a curve of dimension Dim. /// If value of parameter Safe is false, no verification is made on the evaluation of the curve.
/// 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,
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> > struct curve_abc : std::unary_function<Time, Point>, public serialization::Serializable {
struct curve_abc : std::unary_function<Time, Point>,
public serialization::Serializable
{
typedef Point point_t; typedef Point point_t;
typedef Time time_t; typedef Time time_t;
/* Constructors - destructors */ /* Constructors - destructors */
public: public:
/// \brief Constructor. /// \brief Constructor.
curve_abc(){} curve_abc() {}
/// \brief Destructor. /// \brief Destructor.
virtual ~curve_abc(){} virtual ~curve_abc() {}
/* Constructors - destructors */ /* Constructors - destructors */
/*Operations*/ /*Operations*/
...@@ -63,18 +59,17 @@ namespace curves ...@@ -63,18 +59,17 @@ namespace curves
/// \brief Get the maximum time for which the curve is defined. /// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range. /// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const = 0; virtual time_t max() const = 0;
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());} std::pair<time_t, time_t> timeRange() { return std::make_pair(min(), max()); }
/*Helpers*/ /*Helpers*/
// Serialization of the class // Serialization of the class
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template <class Archive>
void serialize(Archive& ar, const unsigned int version){ void serialize(Archive& ar, const unsigned int version) {
if (version) { if (version) {
// Do something depending on version ? // Do something depending on version ?
} }
} }
}; };
} // namespace curves } // namespace curves
#endif //_STRUCT_CURVE_ABC #endif //_STRUCT_CURVE_ABC
/** /**
* \file curve_constraint.h * \file curve_constraint.h
* \brief struct to define constraints on start / end velocities and acceleration * \brief struct to define constraints on start / end velocities and acceleration
* on a curve * on a curve
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 04/05/2017 * \date 04/05/2017
* *
*/ */
#ifndef _CLASS_CURVE_CONSTRAINT #ifndef _CLASS_CURVE_CONSTRAINT
#define _CLASS_CURVE_CONSTRAINT #define _CLASS_CURVE_CONSTRAINT
...@@ -17,20 +16,18 @@ ...@@ -17,20 +16,18 @@
#include <functional> #include <functional>
#include <vector> #include <vector>
namespace curves namespace curves {
{ template <typename Point>
template <typename Point> struct curve_constraints {
struct curve_constraints
{
typedef Point point_t; typedef Point point_t;
curve_constraints(){} curve_constraints() {}
~curve_constraints(){} ~curve_constraints() {}
point_t init_vel; point_t init_vel;
point_t init_acc; point_t init_acc;
point_t end_vel; point_t end_vel;
point_t end_acc; point_t end_acc;
}; };
} // namespace curves } // namespace curves
#endif //_CLASS_CUBICZEROVELACC #endif //_CLASS_CUBICZEROVELACC
...@@ -12,93 +12,88 @@ ...@@ -12,93 +12,88 @@
#include <iostream> #include <iostream>
namespace curves namespace curves {
{ /// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
/// \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.
/// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert. /// \return the equivalent polynomial.
/// \return the equivalent polynomial. template <typename Polynomial, typename curveTypeToConvert>
template<typename Polynomial, typename curveTypeToConvert> Polynomial polynomial_from_curve(const curveTypeToConvert& curve) {
Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
{
typedef typename Polynomial::t_point_t t_point_t; typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t; typedef typename Polynomial::num_t num_t;
t_point_t coefficients; t_point_t coefficients;
curveTypeToConvert current (curve); curveTypeToConvert current(curve);
coefficients.push_back(curve(curve.min())); coefficients.push_back(curve(curve.min()));
num_t T = curve.max()-curve.min(); num_t T = curve.max() - curve.min();
num_t T_div = 1.0; num_t T_div = 1.0;
num_t fact = 1; 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; fact *= (num_t)i;
coefficients.push_back(current.derivate(current.min(),i)/fact); coefficients.push_back(current.derivate(current.min(), i) / fact);
}
return Polynomial(coefficients,curve.min(),curve.max());
} }
return Polynomial(coefficients, curve.min(), curve.max());
}
/// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve. /// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert. /// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve. /// \return the equivalent cubic bezier curve.
template<typename Bezier, typename curveTypeToConvert> template <typename Bezier, typename curveTypeToConvert>
Bezier bezier_from_curve(const curveTypeToConvert& curve) Bezier bezier_from_curve(const curveTypeToConvert& curve) {
{
typedef typename Bezier::point_t point_t; typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t; typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t; typedef typename Bezier::num_t num_t;
curveTypeToConvert current (curve); curveTypeToConvert current(curve);
num_t T_min = current.min(); num_t T_min = current.min();
num_t T_max = current.max(); num_t T_max = current.max();
num_t T = T_max-T_min; num_t T = T_max - T_min;
// Positions and derivatives // Positions and derivatives
point_t p0 = current(T_min); point_t p0 = current(T_min);
point_t p1 = current(T_max); point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min,1); point_t m0 = current.derivate(T_min, 1);
point_t m1 = current.derivate(T_max,1); point_t m1 = current.derivate(T_max, 1);
// Convert to bezier control points // Convert to bezier control points
// for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T // for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T
// so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T // so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T
// <=> b_p1=T(m0/3)+b_p0 and b_p2=-T(m1/3)+b_p3 // <=> b_p1=T(m0/3)+b_p0 and b_p2=-T(m1/3)+b_p3
point_t b_p0 = p0; point_t b_p0 = p0;
point_t b_p3 = p1; point_t b_p3 = p1;
point_t b_p1 = T*m0/3+b_p0; point_t b_p1 = T * m0 / 3 + b_p0;
point_t b_p2 = -T*m1/3+b_p3; point_t b_p2 = -T * m1 / 3 + b_p3;
t_point_t control_points; t_point_t control_points;
control_points.push_back(b_p0); control_points.push_back(b_p0);
control_points.push_back(b_p1); control_points.push_back(b_p1);
control_points.push_back(b_p2); control_points.push_back(b_p2);
control_points.push_back(b_p3); control_points.push_back(b_p3);
return Bezier(control_points.begin(), control_points.end(), current.min(), current.max()); return Bezier(control_points.begin(), control_points.end(), current.min(), current.max());
} }
/// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline. /// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
/// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert. /// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic hermite spline. /// \return the equivalent cubic hermite spline.
template<typename Hermite, typename curveTypeToConvert> template <typename Hermite, typename curveTypeToConvert>
Hermite hermite_from_curve(const curveTypeToConvert& curve) Hermite hermite_from_curve(const curveTypeToConvert& curve) {
{
typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t; typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t;
typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t; typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t;
typedef typename Hermite::point_t point_t; typedef typename Hermite::point_t point_t;
typedef typename Hermite::num_t num_t; typedef typename Hermite::num_t num_t;
curveTypeToConvert current (curve); curveTypeToConvert current(curve);
num_t T_min = current.min(); num_t T_min = current.min();
num_t T_max = current.max(); num_t T_max = current.max();
num_t T = T_max-T_min; num_t T = T_max - T_min;
// Positions and derivatives // Positions and derivatives
point_t p0 = current(T_min); point_t p0 = current(T_min);
point_t p1 = current(T_max); point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min,1); point_t m0 = current.derivate(T_min, 1);
point_t m1 = current.derivate(T_max,1); point_t m1 = current.derivate(T_max, 1);
// Create pairs pos/vel // Create pairs pos/vel
pair_point_tangent_t pair0(p0,m0); pair_point_tangent_t pair0(p0, m0);
pair_point_tangent_t pair1(p1,m1); pair_point_tangent_t pair1(p1, m1);
t_pair_point_tangent_t control_points; t_pair_point_tangent_t control_points;
control_points.push_back(pair0); control_points.push_back(pair0);
control_points.push_back(pair1); control_points.push_back(pair1);
std::vector< double > time_control_points; std::vector<double> time_control_points;
time_control_points.push_back(T_min); time_control_points.push_back(T_min);
time_control_points.push_back(T_max); time_control_points.push_back(T_max);
return Hermite(control_points.begin(), control_points.end(), time_control_points);