Skip to content
Snippets Groups Projects
Commit 88ffc79b authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format]

parent 0e28b340
No related branches found
Tags v0.2.0
No related merge requests found
Pipeline #6108 passed
Stage: test
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 3117 additions and 3474 deletions
include/curves/MathDefs.h
+ 27
− 30
View file @ 88ffc79b
/** /**
* \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) {
{ 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 pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
#endif //_SPLINEMATH }
} // namespace curves
#endif //_SPLINEMATH
include/curves/bernstein.h
+ 56
− 67
View file @ 88ffc79b
/** /**
* \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,72 +17,62 @@ ...@@ -18,72 +17,62 @@
#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 */
Numeric m_minus_i; Numeric m_minus_i;
Numeric i_; Numeric i_;
Numeric bin_m_i_; Numeric bin_m_i_;
/* Attributes */ /* Attributes */
// 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("i", i_);
ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
} }
}; // End struct Bern ar& boost::serialization::make_nvp("m_minus_i", m_minus_i);
ar& boost::serialization::make_nvp("i", i_);
/// \brief Computes all Bernstein polynomes for a certain degree. ar& boost::serialization::make_nvp("bin_m_i", bin_m_i_);
///
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 }; // End struct Bern
#endif //_CLASS_BERNSTEIN
/// \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
include/curves/bezier_curve.h
+ 424
− 472
View file @ 88ffc79b
This diff is collapsed.
include/curves/cubic_hermite_spline.h
+ 348
− 401
View file @ 88ffc79b
This diff is collapsed.
include/curves/cubic_spline.h
+ 34
− 36
View file @ 88ffc79b
/** /**
* \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;
{ res.push_back(a);
T_Point res; res.push_back(b);
res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d); res.push_back(c);
return res; res.push_back(d);
} return res;
}
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, template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point>
const Time t_min, const Time t_max) polynomial<Time, Numeric, 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); 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
include/curves/curve_abc.h
+ 58
− 63
View file @ 88ffc79b
/** /**
* \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,63 +17,59 @@ ...@@ -18,63 +17,59 @@
#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>, typedef Point point_t;
public serialization::Serializable typedef Time time_t;
{
typedef Point point_t;
typedef Time time_t;
/* Constructors - destructors */
public:
/// \brief Constructor.
curve_abc(){}
/// \brief Destructor. /* Constructors - destructors */
virtual ~curve_abc(){} public:
/* Constructors - destructors */ /// \brief Constructor.
curve_abc() {}
/*Operations*/ /// \brief Destructor.
/// \brief Evaluation of the cubic spline at time t. virtual ~curve_abc() {}
/// \param t : time when to evaluate the spine /* Constructors - destructors */
/// \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. /*Operations*/
/// \param t : time when to evaluate the spline. /// \brief Evaluation of the cubic spline at time t.
/// \param order : order of derivative. /// \param t : time when to evaluate the spine
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t. /// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t derivate(const time_t t, const std::size_t order) const = 0; virtual point_t operator()(const time_t t) const = 0;
/*Operations*/
/*Helpers*/ /// \brief Evaluate the derivative of order N of curve at time t.
/// \brief Get dimension of curve. /// \param t : time when to evaluate the spline.
/// \return dimension of curve. /// \param order : order of derivative.
virtual std::size_t dim() const = 0; /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
/// \brief Get the minimum time for which the curve is defined. virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
/// \return \f$t_{min}\f$, lower bound of time range. /*Operations*/
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*/
// Serialization of the class /*Helpers*/
friend class boost::serialization::access; /// \brief Get dimension of curve.
template<class Archive> /// \return dimension of curve.
void serialize(Archive& ar, const unsigned int version){ virtual std::size_t dim() const = 0;
if (version) { /// \brief Get the minimum time for which the curve is defined.
// Do something depending on version ? /// \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.
} // namespace curves virtual time_t max() const = 0;
#endif //_STRUCT_CURVE_ABC std::pair<time_t, time_t> timeRange() { return std::make_pair(min(), max()); }
/*Helpers*/
// 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 ?
}
}
};
} // namespace curves
#endif //_STRUCT_CURVE_ABC
include/curves/curve_constraint.h
+ 21
− 24
View file @ 88ffc79b
/** /**
* \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;
{ curve_constraints() {}
typedef Point point_t;
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
include/curves/curve_conversion.h
+ 82
− 87
View file @ 88ffc79b
...@@ -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::num_t num_t;
typedef typename Polynomial::t_point_t t_point_t; t_point_t coefficients;
typedef typename Polynomial::num_t num_t; curveTypeToConvert current(curve);
t_point_t coefficients; coefficients.push_back(curve(curve.min()));
curveTypeToConvert current (curve); num_t T = curve.max() - curve.min();
coefficients.push_back(curve(curve.min())); num_t T_div = 1.0;
num_t T = curve.max()-curve.min(); num_t fact = 1;
num_t T_div = 1.0; for (std::size_t i = 1; i <= curve.degree_; ++i) {
num_t fact = 1; fact *= (num_t)i;
for(std::size_t i = 1; i<= curve.degree_; ++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());
} }
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);
return Hermite(control_points.begin(), control_points.end(), time_control_points); }
} } // namespace curves
} // namespace curve #endif //_CLASS_CURVE_CONVERSION
#endif //_CLASS_CURVE_CONVERSION \ No newline at end of file
\ No newline at end of file
include/curves/exact_cubic.h
+ 228
− 243
View file @ 88ffc79b
This diff is collapsed.
include/curves/helpers/effector_spline.h
+ 103
− 110
View file @ 88ffc79b
/** /**
* \file exact_cubic.h * \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline. * \brief class allowing to create an Exact 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 ExactCubic class. * This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions : * cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ; * - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1 * - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1 * - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization" * more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009) * By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/ */
#ifndef _CLASS_EFFECTORSPLINE #ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE #define _CLASS_EFFECTORSPLINE
#include "curves/exact_cubic.h" #include "curves/exact_cubic.h"
namespace curves namespace curves {
{ namespace helpers {
namespace helpers typedef double Numeric;
{ typedef double Time;
typedef double Numeric; typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> Point;
typedef double Time; typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> Point; typedef std::pair<double, Point> Waypoint;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point; typedef std::vector<Waypoint> T_Waypoint;
typedef std::pair<double, Point> Waypoint; typedef exact_cubic<Time, Numeric, true, Point, T_Point> exact_cubic_t;
typedef std::vector<Waypoint> T_Waypoint; typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef exact_cubic<Time, Numeric, true, Point, T_Point> exact_cubic_t; typedef exact_cubic_t::t_spline_t t_spline_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t; typedef exact_cubic_t::spline_t spline_t;
typedef exact_cubic_t::t_spline_t t_spline_t;
typedef exact_cubic_t::spline_t spline_t;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
{
Numeric norm = normal.norm();
assert(norm>0.);
return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
}
/// \brief Compute spline from land way point to end point. /// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
/// Constraints are null velocity and acceleration. Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, Numeric norm = normal.norm();
const Time init_time, const Time time_offset) assert(norm > 0.);
{ return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
Numeric norm = normal.norm(); }
assert(norm>0.);
Point n = normal / norm;
Point d = offset / (time_offset*time_offset*time_offset) * -n;
Point c = -3 * d * time_offset;
Point b = -c * time_offset;
T_Point points;
points.push_back(from);
points.push_back(b);
points.push_back(c);
points.push_back(d);
return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
}
/// \brief Compute end velocity : along landing normal and respecting time. /// \brief Compute spline from land way point to end point.
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/) /// Constraints are null velocity and acceleration.
{ spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time,
spline_constraints_t constraints; const Time time_offset) {
constraints.init_acc = Point::Zero(end_spline.dim()); Numeric norm = normal.norm();
constraints.init_vel = Point::Zero(end_spline.dim()); assert(norm > 0.);
constraints.end_acc = end_spline.derivate(end_spline.min(),2); Point n = normal / norm;
constraints.end_vel = end_spline.derivate(end_spline.min(),1); Point d = offset / (time_offset * time_offset * time_offset) * -n;
return constraints; Point c = -3 * d * time_offset;
} Point b = -c * time_offset;
T_Point points;
points.push_back(from);
points.push_back(b);
points.push_back(c);
points.push_back(d);
return spline_t(points.begin(), points.end(), init_time, init_time + time_offset);
}
/// \brief Compute end velocity : along landing normal and respecting time.
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline,
const Time /*time_offset*/) {
spline_constraints_t constraints;
constraints.init_acc = Point::Zero(end_spline.dim());
constraints.init_vel = Point::Zero(end_spline.dim());
constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
constraints.end_vel = end_spline.derivate(end_spline.min(), 1);
return constraints;
}
/// \brief Helper method to create a spline typically used to /// \brief Helper method to create a spline typically used to
/// guide the 3d trajectory of a robot end effector. /// guide the 3d trajectory of a robot end effector.
/// Given a set of waypoints, and the normal vector of the start and /// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that: /// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0. /// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals. /// + the effector lifts and lands exactly in the direction of the specified normals.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container. /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEnd : an iterator pointing to the last element of a waypoint container. /// \param wayPointsEnd : an iterator pointing to the last element of a waypoint container.
/// \param lift_normal : normal to be followed by end effector at take-off. /// \param lift_normal : normal to be followed by end effector at take-off.
/// \param land_normal : normal to be followed by end effector at landing. /// \param land_normal : normal to be followed by end effector at landing.
/// \param lift_offset : length of the straight line along normal at take-off. /// \param lift_offset : length of the straight line along normal at take-off.
/// \param land_offset : length of the straight line along normal at landing. /// \param land_offset : length of the straight line along normal at landing.
/// \param lift_offset_duration : time travelled along straight line at take-off. /// \param lift_offset_duration : time travelled along straight line at take-off.
/// \param land_offset_duration : time travelled along straight line at landing. /// \param land_offset_duration : time travelled along straight line at landing.
/// ///
template<typename In> template <typename In>
exact_cubic_t* effector_spline(In wayPointsBegin, In wayPointsEnd, exact_cubic_t* effector_spline(In wayPointsBegin, In wayPointsEnd, const Point& lift_normal = Eigen::Vector3d::UnitZ(),
const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal = Eigen::Vector3d::UnitZ(), const Numeric lift_offset = 0.02,
const Point& land_normal=Eigen::Vector3d::UnitZ(), const Numeric land_offset = 0.02, const Time lift_offset_duration = 0.02,
const Numeric lift_offset=0.02, const Numeric land_offset=0.02, const Time land_offset_duration = 0.02) {
const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02) T_Waypoint waypoints;
{ const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
T_Waypoint waypoints; waypoints.push_back(inPoint);
const Waypoint& inPoint=*wayPointsBegin, endPoint =*(wayPointsEnd-1); // adding initial offset
waypoints.push_back(inPoint); waypoints.push_back(compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
//adding initial offset // inserting all waypoints but last
waypoints.push_back(compute_offset(inPoint, lift_normal ,lift_offset, lift_offset_duration)); waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
//inserting all waypoints but last // inserting waypoint to start landing
waypoints.insert(waypoints.end(),wayPointsBegin+1,wayPointsEnd-1); const Waypoint& landWaypoint = compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
//inserting waypoint to start landing waypoints.push_back(landWaypoint);
const Waypoint& landWaypoint=compute_offset(endPoint, land_normal ,land_offset, -land_offset_duration); // specifying end velocity constraint such that landing will be in straight line
waypoints.push_back(landWaypoint); spline_t end_spline =
//specifying end velocity constraint such that landing will be in straight line make_end_spline(land_normal, landWaypoint.second, land_offset, landWaypoint.first, land_offset_duration);
spline_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,land_offset_duration); spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline, land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration); exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(), constraints);
exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(),constraints); t_spline_t splines = all_but_end.curves_;
t_spline_t splines = all_but_end.curves_; splines.push_back(end_spline);
splines.push_back(end_spline); return new exact_cubic_t(splines);
return new exact_cubic_t(splines); }
} } // namespace helpers
} // namespace helpers } // namespace curves
} // namespace curves #endif //_CLASS_EFFECTORSPLINE
#endif //_CLASS_EFFECTORSPLINE
include/curves/helpers/effector_spline_rotation.h
+ 237
− 252
View file @ 88ffc79b
This diff is collapsed.
include/curves/linear_variable.h
+ 166
− 212
View file @ 88ffc79b
/** /**
* \file linear_variable.h * \file linear_variable.h
* \brief storage for variable points of the form p_i = a_i x + b_i * \brief storage for variable points of the form p_i = a_i x + b_i
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 07/02/2019 * \date 07/02/2019
*/ */
#ifndef _CLASS_LINEAR_VARIABLE #ifndef _CLASS_LINEAR_VARIABLE
#define _CLASS_LINEAR_VARIABLE #define _CLASS_LINEAR_VARIABLE
...@@ -19,231 +18,186 @@ ...@@ -19,231 +18,186 @@
#include <Eigen/Core> #include <Eigen/Core>
#include <stdexcept> #include <stdexcept>
namespace curves namespace curves {
{ template <int Dim, typename Numeric = double>
template <int Dim, typename Numeric=double> struct linear_variable {
struct linear_variable{ typedef Numeric num_t;
typedef Numeric num_t; typedef Eigen::Matrix<num_t, Dim, Dim> matrix_t;
typedef Eigen::Matrix<num_t, Dim, Dim> matrix_t; typedef Eigen::Matrix<num_t, Dim, 1> point_t;
typedef Eigen::Matrix<num_t, Dim, 1> point_t; typedef linear_variable<Dim, Numeric> linear_variable_t;
typedef linear_variable<Dim, Numeric> linear_variable_t;
/* Attributes */
/* Attributes */ matrix_t A_;
matrix_t A_; point_t b_;
point_t b_; /* Attributes */
/* Attributes */
linear_variable() : A_(matrix_t::Identity()), b_(point_t::Zero()) {}
linear_variable() linear_variable(const matrix_t& A, const point_t& b) : A_(A), b_(b) {}
: A_(matrix_t::Identity()), b_(point_t::Zero()) linear_variable(const point_t& b) : A_(matrix_t::Zero()), b_(b) {} // constant
{}
linear_variable(const matrix_t& A, const point_t& b) linear_variable& operator+=(const linear_variable& w1) {
: A_(A),b_(b) this->A_ += w1.A_;
{} this->b_ += w1.b_;
linear_variable(const point_t& b) return *this;
: A_(matrix_t::Zero()),b_(b) }
{} // constant
linear_variable& operator+=(const linear_variable& w1)
{
this->A_ += w1.A_;
this->b_ += w1.b_;
return *this;
}
linear_variable& operator-=(const linear_variable& w1) linear_variable& operator-=(const linear_variable& w1) {
{ this->A_ -= w1.A_;
this->A_ -= w1.A_; this->b_ -= w1.b_;
this->b_ -= w1.b_; return *this;
return *this; }
}
static linear_variable_t Zero(){ static linear_variable_t Zero() {
linear_variable_t w; linear_variable_t w;
w.A_ = matrix_t::Zero(); w.A_ = matrix_t::Zero();
w.b_ = point_t::Zero(); w.b_ = point_t::Zero();
return w; return w;
} }
}; // End struct linear_variable }; // End struct linear_variable
template<typename Var>
struct variables
{
typedef Var var_t;
typedef variables<Var> variables_t;
typedef std::vector<var_t> T_var_t;
typedef typename T_var_t::iterator IT_var_t;
typedef typename T_var_t::const_iterator CIT_var_t;
T_var_t variables_;
variables() {}
variables& operator+=(const variables& w1)
{
if(variables_.size() == 0)
{
variables_ = w1.variables_;
}
else if (w1.variables_.size() !=0)
{
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
{
(*it)+=(*cit);
}
}
return *this;
}
variables& operator-=(const variables& w1) template <typename Var>
{ struct variables {
if(variables_.size() == 0) typedef Var var_t;
{ typedef variables<Var> variables_t;
variables_ = w1.variables_;
}
else if (w1.variables_.size() !=0)
{
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
{
(*it)-=(*cit);
}
}
return *this;
}
std::size_t size() typedef std::vector<var_t> T_var_t;
{ typedef typename T_var_t::iterator IT_var_t;
variables_t w; typedef typename T_var_t::const_iterator CIT_var_t;
return w.size();
}
static variables_t Zero(size_t /*dim*/){ T_var_t variables_;
variables_t w;
return w;
}
}; // End struct variables
template <int D, typename N> variables() {}
inline linear_variable<D,N> operator+(const linear_variable<D,N>& w1, const linear_variable<D,N>& w2)
{
return linear_variable<D,N>(w1.A_ + w2.A_, w1.b_ + w2.b_);
}
template <int D, typename N> variables& operator+=(const variables& w1) {
linear_variable<D,N> operator-(const linear_variable<D,N>& w1, const linear_variable<D,N>& w2) if (variables_.size() == 0) {
{ variables_ = w1.variables_;
return linear_variable<D,N>(w1.A_ - w2.A_, w1.b_ - w2.b_); } else if (w1.variables_.size() != 0) {
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for (IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit) {
(*it) += (*cit);
}
}
return *this;
} }
template <int D, typename N> variables& operator-=(const variables& w1) {
linear_variable<D,N> operator*(const double k, const linear_variable<D,N>& w) if (variables_.size() == 0) {
{ variables_ = w1.variables_;
return linear_variable<D,N>(k*w.A_,k*w.b_); } else if (w1.variables_.size() != 0) {
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for (IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit) {
(*it) -= (*cit);
}
}
return *this;
} }
template <int D, typename N> std::size_t size() {
linear_variable<D,N> operator*(const linear_variable<D,N>& w,const double k) variables_t w;
{ return w.size();
return linear_variable<D,N>(k*w.A_,k*w.b_);
} }
template <int D, typename N> static variables_t Zero(size_t /*dim*/) {
linear_variable<D,N> operator/(const linear_variable<D,N>& w,const double k) variables_t w;
{ return w;
return linear_variable<D,N>(w.A_/k,w.b_/k);
} }
}; // End struct variables
template<typename V>
variables<V> operator+(const variables<V>& w1, const variables<V>& w2) template <int D, typename N>
{ inline linear_variable<D, N> operator+(const linear_variable<D, N>& w1, const linear_variable<D, N>& w2) {
if(w2.variables_.size() == 0) return linear_variable<D, N>(w1.A_ + w2.A_, w1.b_ + w2.b_);
{ }
return w1;
} template <int D, typename N>
if(w1.variables_.size() == 0) linear_variable<D, N> operator-(const linear_variable<D, N>& w1, const linear_variable<D, N>& w2) {
{ return linear_variable<D, N>(w1.A_ - w2.A_, w1.b_ - w2.b_);
return w2; }
}
variables<V> res; template <int D, typename N>
assert(w2.variables_.size() == w1.variables_.size()); linear_variable<D, N> operator*(const double k, const linear_variable<D, N>& w) {
typename variables<V>::CIT_var_t cit = w1.variables_.begin(); return linear_variable<D, N>(k * w.A_, k * w.b_);
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2) }
{
res.variables_.push_back((*cit)+(*cit2)); template <int D, typename N>
} linear_variable<D, N> operator*(const linear_variable<D, N>& w, const double k) {
return res; return linear_variable<D, N>(k * w.A_, k * w.b_);
}
template <int D, typename N>
linear_variable<D, N> operator/(const linear_variable<D, N>& w, const double k) {
return linear_variable<D, N>(w.A_ / k, w.b_ / k);
}
template <typename V>
variables<V> operator+(const variables<V>& w1, const variables<V>& w2) {
if (w2.variables_.size() == 0) {
return w1;
} }
if (w1.variables_.size() == 0) {
template<typename V> return w2;
variables<V> operator-(const variables<V>& w1, const variables<V>& w2) }
{ variables<V> res;
if(w2.variables_.size() == 0) assert(w2.variables_.size() == w1.variables_.size());
{ typename variables<V>::CIT_var_t cit = w1.variables_.begin();
return w1; for (typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2) {
} res.variables_.push_back((*cit) + (*cit2));
if(w1.variables_.size() == 0)
{
return w2;
}
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
{
res.variables_.push_back((*cit)-(*cit2));
}
return res;
} }
return res;
}
template<typename V> template <typename V>
variables<V> operator*(const double k, const variables<V>& w) variables<V> operator-(const variables<V>& w1, const variables<V>& w2) {
{ if (w2.variables_.size() == 0) {
if(w.variables_.size() == 0) return w1;
{ }
return w; if (w1.variables_.size() == 0) {
} return w2;
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
{
res.variables_.push_back(k*(*cit));
}
return res;
} }
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for (typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2) {
res.variables_.push_back((*cit) - (*cit2));
}
return res;
}
template<typename V> template <typename V>
variables<V> operator*(const variables<V>& w,const double k) variables<V> operator*(const double k, const variables<V>& w) {
{ if (w.variables_.size() == 0) {
if(w.variables_.size() == 0) return w;
{
return w;
}
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
{
res.variables_.push_back((*cit)*k);
}
return res;
} }
variables<V> res;
for (typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit) {
res.variables_.push_back(k * (*cit));
}
return res;
}
template<typename V> template <typename V>
variables<V> operator/(const variables<V>& w,const double k) variables<V> operator*(const variables<V>& w, const double k) {
{ if (w.variables_.size() == 0) {
if(w.variables_.size() == 0) return w;
{ }
return w; variables<V> res;
} for (typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit) {
variables<V> res; res.variables_.push_back((*cit) * k);
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
{
res.variables_.push_back((*cit)/k);
}
return res;
} }
} // namespace curves return res;
#endif //_CLASS_LINEAR_VARIABLE }
template <typename V>
variables<V> operator/(const variables<V>& w, const double k) {
if (w.variables_.size() == 0) {
return w;
}
variables<V> res;
for (typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit) {
res.variables_.push_back((*cit) / k);
}
return res;
}
} // namespace curves
#endif //_CLASS_LINEAR_VARIABLE
include/curves/optimization/OptimizeSpline.h
+ 440
− 482
View file @ 88ffc79b
This diff is collapsed.
include/curves/piecewise_curve.h
+ 357
− 383
View file @ 88ffc79b
This diff is collapsed.
include/curves/polynomial.h
+ 353
− 402
View file @ 88ffc79b
This diff is collapsed.
include/curves/quintic_spline.h
+ 38
− 38
View file @ 88ffc79b
/** /**
* \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_QUINTIC_SPLINE #ifndef _STRUCT_QUINTIC_SPLINE
#define _STRUCT_QUINTIC_SPLINE #define _STRUCT_QUINTIC_SPLINE
...@@ -20,30 +19,31 @@ ...@@ -20,30 +19,31 @@
#include <stdexcept> #include <stdexcept>
namespace curves namespace curves {
{ /// \brief Creates coefficient vector of a quintic spline defined on the interval
/// \brief Creates coefficient vector of a quintic 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 + e(t - t_{min})^4 + f(t - t_{min})^5 \f$ <br>
/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 + e(t - t_{min})^4 + f(t - t_{min})^5 \f$ <br> /// where \f$ t \in [t_{min}, t_{max}] \f$.
/// where \f$ t \in [t_{min}, t_{max}] \f$. ///
/// template <typename Point, typename T_Point>
template<typename Point, typename T_Point> T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c, Point const& d, Point const& e,
T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c, Point const& f) {
Point const &d, Point const& e, Point const& f) T_Point res;
{ res.push_back(a);
T_Point res; res.push_back(b);
res.push_back(a);res.push_back(b);res.push_back(c); res.push_back(c);
res.push_back(d);res.push_back(e);res.push_back(f); res.push_back(d);
return res; res.push_back(e);
} res.push_back(f);
return res;
template<typename Time, typename Numeric, bool Safe, typename Point, typename T_Point> }
polynomial<Time,Numeric,Safe,Point,T_Point> create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
const Time t_min, const Time t_max) template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point>
{ polynomial<Time, Numeric, Safe, Point, T_Point> create_quintic(Point const& a, Point const& b, Point const& c,
T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f); Point const& d, Point const& e, Point const& f,
return polynomial<Time,Numeric,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max); const Time t_min, const Time t_max) {
} T_Point coeffs = make_quintic_vector<Point, T_Point>(a, b, c, d, e, f);
} // namespace curves return polynomial<Time, Numeric, Safe, Point, T_Point>(coeffs.begin(), coeffs.end(), t_min, t_max);
#endif //_STRUCT_QUINTIC_SPLINE }
} // namespace curves
#endif //_STRUCT_QUINTIC_SPLINE
include/curves/serialization/archive.hpp
+ 91
− 115
View file @ 88ffc79b
This diff is collapsed.
include/curves/serialization/eigen-matrix.hpp
+ 31
− 29
View file @ 88ffc79b
This diff is collapsed.
include/curves/serialization/fwd.hpp
+ 2
− 2
View file @ 88ffc79b
...@@ -6,6 +6,6 @@ ...@@ -6,6 +6,6 @@
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#define BOOST_SERIALIZATION_MAKE_NVP(member) boost::serialization::make_nvp(##member,member) #define BOOST_SERIALIZATION_MAKE_NVP(member) boost::serialization::make_nvp(##member, member)
#endif // ifndef __multicontact_api_serialization_fwd_hpp__ #endif // ifndef __multicontact_api_serialization_fwd_hpp__
python/archive_python_binding.h
+ 21
− 26
View file @ 88ffc79b
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment