Skip to content

GitLab

Commit 13089d03 authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[Indentation] Reindentation of all the code

parent c55a3f93
Expand all Hide whitespace changes
Inline Side-by-side
include/curves/MathDefs.h
View file @ 13089d03
......@@ -21,26 +21,22 @@
#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)
{
//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);
}
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
include/curves/bernstein.h
View file @ 13089d03
......@@ -22,71 +22,71 @@
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);
/// \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 : public serialization::Serializable< Bern<Numeric> > {
Bern(){}
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i)
,i_(i)
,bin_m_i_(bin(m,i)) {}
~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 m_minus_i;
Numeric i_;
Numeric bin_m_i_;
// Serialization of the class
friend class boost::serialization::access;
}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template <typename Numeric = double>
struct Bern : public serialization::Serializable< Bern<Numeric> > {
Bern(){}
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i),
i_(i),
bin_m_i_(bin(m,i))
{}
~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);
}
template<class Archive>
void serialize(Archive& ar, const unsigned int version){
if (version) {
// Do something depending on version ?
/* 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) {
// 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_);
}
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)
{
/// \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));
{
res.push_back(Bern<Numeric>(n, i));
}
return res;
}
}
} // namespace curves
#endif //_CLASS_BERNSTEIN
include/curves/bezier_curve.h
View file @ 13089d03
This diff is collapsed.
include/curves/bezier_polynomial_conversion.h deleted 100644 → 0
View file @ c55a3f93
/**
* \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 _BEZIER_POLY_CONVERSION
#define _BEZIER_POLY_CONVERSION
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept>
#include <iostream>
namespace curves
{
/// \brief Converts a Bezier curve to a polynomial.
/// \param bezier : the Bezier curve to convert.
/// \return the equivalent polynomial.
template<typename Bezier, typename Polynomial>
Polynomial from_bezier(const Bezier& curve)
{
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
t_point_t coefficients;
Bezier current (curve);
coefficients.push_back(curve(0.));
num_t fact = 1;
for(std::size_t i = 1; i<= curve.degree_; ++i)
{
current = current.compute_derivate(1);
fact *= (num_t)i;
coefficients.push_back(current(0.)/fact);
}
return Polynomial(coefficients,curve.min(),curve.max());
}
/*
/// \brief Converts a polynomial to a Bezier curve.
/// \param polynomial : the polynomial to convert.
/// \return the equivalent Bezier curve.
template<typename Bezier, typename Polynomial>
Bezier from_polynomial(const Polynomial& polynomial)
{
typedef Bezier::point_t point_t;
typedef Bezier::time_t time_t;
typedef Bezier::num_t num_t;
typedef Bezier::curve_constraints_t curve_constraints_t;
typedef Bezier::t_point_t t_point_t;
typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t;
}
*/
} // namespace curves
#endif //_BEZIER_POLY_CONVERSION
include/curves/cubic_hermite_spline.h
View file @ 13089d03
This diff is collapsed.
include/curves/cubic_spline.h
View file @ 13089d03
......@@ -22,26 +22,27 @@
namespace curves
{
/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \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$
/// with a, b, c and d the control points.
///
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 res;
res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
return res;
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
polynomial<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time t_min, const Time t_max)
{
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d);
return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
}
/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \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$
/// with a, b, c and d the control points.
///
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 res;
res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
return res;
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe,
typename Point, typename T_Point>
polynomial<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time t_min, const Time t_max)
{
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d);
return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
}
} // namespace curves
#endif //_STRUCT_CUBICSPLINE
include/curves/curve_abc.h
View file @ 13089d03
......@@ -20,53 +20,49 @@
namespace curves
{
/// \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> >
struct curve_abc : std::unary_function<Time, Point>
{
/// \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> >
struct curve_abc : std::unary_function<Time, Point>
{
typedef Point point_t;
typedef Time time_t;
/* Constructors - destructors */
/* Constructors - destructors */
public:
/// \brief Constructor.
curve_abc(){}
/// \brief Destructor.
virtual ~curve_abc(){}
/* Constructors - destructors */
/// \brief Constructor.
curve_abc(){}
/*Operations*/
public:
/// \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 = 0;
/// \brief Destructor.
virtual ~curve_abc(){}
/* 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 = 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;
/*Operations*/
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const = 0;
/// \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 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;
/*Operations*/
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
/*Helpers*/
/*Helpers*/
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const = 0;
/// \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;
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
/*Helpers*/
};
};
} // namespace curves
#endif //_STRUCT_CURVE_ABC
include/curves/curve_constraint.h
View file @ 13089d03
......@@ -19,19 +19,19 @@
namespace curves
{
template <typename Point, std::size_t Dim=3>
struct curve_constraints
{
typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
template <typename Point, std::size_t Dim=3>
struct curve_constraints
{
typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
~curve_constraints(){}
~curve_constraints(){}
point_t init_vel;
point_t init_acc;
point_t end_vel;
point_t end_acc;
};
point_t init_vel;
point_t init_acc;
point_t end_vel;
point_t end_acc;
};
} // namespace curves
#endif //_CLASS_CUBICZEROVELACC
include/curves/curve_conversion.h
View file @ 13089d03
......@@ -14,13 +14,12 @@
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)
{
/// \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)
{
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
t_point_t coefficients;
......@@ -31,34 +30,30 @@ Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
num_t fact = 1;
for(std::size_t i = 1; i<= curve.degree_; ++i)
{
fact *= (num_t)i;
coefficients.push_back(current.derivate(current.min(),i)/fact);
fact *= (num_t)i;
coefficients.push_back(current.derivate(current.min(),i)/fact);
}
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.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve.
template<typename Bezier, typename curveTypeToConvert>
Bezier bezier_from_curve(const curveTypeToConvert& 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.
/// \return the equivalent cubic bezier curve.
template<typename Bezier, typename curveTypeToConvert>
Bezier bezier_from_curve(const curveTypeToConvert& curve)
{
typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t;
curveTypeToConvert current (curve);
num_t T_min = current.min();
num_t T_max = current.max();
num_t T = T_max-T_min;
// Positions and derivatives
point_t p0 = current(T_min);
point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min,1);
point_t m1 = current.derivate(T_max,1);
// 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
// so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T
......@@ -67,51 +62,43 @@ Bezier bezier_from_curve(const curveTypeToConvert& curve)
point_t b_p3 = p1;
point_t b_p1 = T*m0/3+b_p0;
point_t b_p2 = -T*m1/3+b_p3;
t_point_t control_points;
control_points.push_back(b_p0);
control_points.push_back(b_p1);
control_points.push_back(b_p2);
control_points.push_back(b_p3);
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.
/// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic hermite spline.
template<typename Hermite, typename curveTypeToConvert>
Hermite hermite_from_curve(const curveTypeToConvert& curve)
{
}
/// \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.
/// \return the equivalent cubic hermite spline.
template<typename Hermite, typename curveTypeToConvert>
Hermite hermite_from_curve(const curveTypeToConvert& curve)
{
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::point_t point_t;
typedef typename Hermite::num_t num_t;
curveTypeToConvert current (curve);
num_t T_min = current.min();
num_t T_max = current.max();
num_t T = T_max-T_min;
// Positions and derivatives
point_t p0 = current(T_min);
point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min,1);
point_t m1 = current.derivate(T_max,1);
// Create pairs pos/vel
pair_point_tangent_t pair0(p0,m0);
pair_point_tangent_t pair1(p1,m1);
t_pair_point_tangent_t control_points;
control_points.push_back(pair0);
control_points.push_back(pair1);
std::vector< double > time_control_points;
time_control_points.push_back(T_min);
time_control_points.push_back(T_max);
return Hermite(control_points.begin(), control_points.end(), time_control_points);
}
}
} // namespace curve
#endif //_CLASS_CURVE_CONVERSION
\ No newline at end of file
include/curves/exact_cubic.h
View file @ 13089d03
......@@ -37,16 +37,17 @@
namespace curves
{
/// \class ExactCubic.
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >
, typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>//,
//public serialization::Serializable< exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> >
{
/// \class ExactCubic.
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >,
typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>//,
//public serialization::Serializable< exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> >
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
......@@ -65,68 +66,67 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
/* Constructors - destructors */
public:
exact_cubic(){}
exact_cubic(){}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
{}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
/// \param constraints : constraints on the init and end velocity / accelerations of the spline.
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
{}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
/// \param constraints : constraints on the init and end velocity / accelerations of the spline.
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
: piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints))
{}
{}
/// \brief Constructor.
/// \param subSplines: vector of subSplines.
exact_cubic(const t_spline_t& subSplines)