hpp-spline  4.10.0
template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics.
polynom.h
Go to the documentation of this file.
1 
13 #ifndef _STRUCT_POLYNOM
14 #define _STRUCT_POLYNOM
15 
16 #include "MathDefs.h"
17 
18 #include "curve_abc.h"
19 
20 #include <iostream>
21 #include <algorithm>
22 #include <functional>
23 #include <stdexcept>
24 
25 namespace spline {
31 template <typename Time = double, typename Numeric = Time, std::size_t Dim = 3, bool Safe = false,
32  typename Point = Eigen::Matrix<Numeric, Dim, 1>,
33  typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> > >
34 struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point> {
35  typedef Point point_t;
36  typedef T_Point t_point_t;
37  typedef Time time_t;
38  typedef Numeric num_t;
40  typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
41  typedef Eigen::Ref<coeff_t> coeff_t_ref;
42 
43  /* Constructors - destructors */
44  public:
51  polynom(const coeff_t& coefficients, const time_t min, const time_t max)
52  : curve_abc_t(),
53  coefficients_(coefficients),
54  dim_(Dim),
55  order_(coefficients_.cols() - 1),
56  t_min_(min),
57  t_max_(max) {
58  safe_check();
59  }
60 
67  polynom(const T_Point& coefficients, const time_t min, const time_t max)
68  : curve_abc_t(),
69  coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
70  dim_(Dim),
71  order_(coefficients_.cols() - 1),
72  t_min_(min),
73  t_max_(max) {
74  safe_check();
75  }
76 
83  template <typename In>
84  polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
85  : coefficients_(init_coeffs(zeroOrderCoefficient, out)),
86  dim_(Dim),
87  order_(coefficients_.cols() - 1),
88  t_min_(min),
89  t_max_(max) {
90  safe_check();
91  }
92 
95  // NOTHING
96  }
97 
98  polynom(const polynom& other)
100  dim_(other.dim_),
101  order_(other.order_),
102  t_min_(other.t_min_),
103  t_max_(other.t_max_) {}
104 
105  // polynom& operator=(const polynom& other);
106 
107  private:
108  void safe_check() {
109  if (Safe) {
110  if (t_min_ > t_max_) std::out_of_range("TODO");
111  if (coefficients_.size() != int(order_ + 1)) std::runtime_error("Spline order and coefficients do not match");
112  }
113  }
114 
115  /* Constructors - destructors */
116 
117  /*Operations*/
118  public:
119  /*/// \brief Evaluation of the cubic spline at time t.
122  virtual point_t operator()(const time_t t) const
123  {
124  if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
125  time_t const dt (t-t_min_);
126  time_t cdt(1);
127  point_t currentPoint_ = point_t::Zero();
128  for(int i = 0; i < order_+1; ++i, cdt*=dt)
129  currentPoint_ += cdt *coefficients_.col(i);
130  return currentPoint_;
131  }*/
132 
136  virtual point_t operator()(const time_t t) const {
137  if ((t < t_min_ || t > t_max_) && Safe) {
138  throw std::out_of_range("TODO");
139  }
140  time_t const dt(t - t_min_);
141  point_t h = coefficients_.col(order_);
142  for (int i = (int)(order_ - 1); i >= 0; i--) h = dt * h + coefficients_.col(i);
143  return h;
144  }
145 
150  virtual point_t derivate(const time_t t, const std::size_t order) const {
151  if ((t < t_min_ || t > t_max_) && Safe) {
152  throw std::out_of_range("TODO");
153  }
154  time_t const dt(t - t_min_);
155  time_t cdt(1);
156  point_t currentPoint_ = point_t::Zero();
157  for (int i = (int)(order); i < (int)(order_ + 1); ++i, cdt *= dt)
158  currentPoint_ += cdt * coefficients_.col(i) * fact(i, order);
159  return currentPoint_;
160  }
161 
162  private:
163  num_t fact(const std::size_t n, const std::size_t order) const {
164  num_t res(1);
165  for (std::size_t i = 0; i < order; ++i) res *= (num_t)(n - i);
166  return res;
167  }
168 
169  /*Operations*/
170 
171  /*Helpers*/
172  public:
174  num_t virtual min() const { return t_min_; }
176  num_t virtual max() const { return t_max_; }
177  /*Helpers*/
178 
179  /*Attributes*/
180  public:
182  std::size_t dim_; // const
183  std::size_t order_; // const
184 
185  private:
186  time_t t_min_, t_max_;
187  /*Attributes*/
188 
189  private:
190  template <typename In>
191  coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
192  std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
193  coeff_t res = coeff_t(Dim, size);
194  int i = 0;
195  for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i) res.col(i) = *cit;
196  return res;
197  }
198 }; // class polynom
199 } // namespace spline
200 #endif //_STRUCT_POLYNOM
spline::polynom::dim_
std::size_t dim_
Definition: polynom.h:182
spline::polynom::order_
std::size_t order_
Definition: polynom.h:183
spline::polynom::polynom
polynom(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:51
spline::polynom::polynom
polynom(const polynom &other)
Definition: polynom.h:98
spline::polynom::point_t
Point point_t
Definition: polynom.h:35
spline::polynom::~polynom
~polynom()
Destructor.
Definition: polynom.h:94
spline::polynom::operator()
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner's scheme.
Definition: polynom.h:136
spline::polynom::t_point_t
T_Point t_point_t
Definition: polynom.h:36
spline::polynom::coefficients_
coeff_t coefficients_
Definition: polynom.h:181
spline::polynom::num_t
Numeric num_t
Definition: polynom.h:38
spline::polynom::min
virtual num_t min() const
Returns the minimum time for wich curve is defined.
Definition: polynom.h:174
spline::helpers::Numeric
double Numeric
Definition: effector_spline.h:26
spline::helpers::Time
double Time
Definition: effector_spline.h:27
curve_abc.h
interface for a Curve of arbitrary dimension.
spline::polynom::max
virtual num_t max() const
Returns the maximum time for wich curve is defined.
Definition: polynom.h:176
spline::helpers::Point
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
spline::polynom
Represents a polynomf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equation...
Definition: polynom.h:34
MathDefs.h
spline::polynom::polynom
polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:84
spline::polynom::derivate
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative spline at time t.
Definition: polynom.h:150
spline::curve_abc
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
spline::polynom::time_t
Time time_t
Definition: polynom.h:37
spline::polynom::coeff_t
Eigen::Matrix< double, Dim, Eigen::Dynamic > coeff_t
Definition: polynom.h:40
spline::polynom::curve_abc_t
curve_abc< Time, Numeric, Dim, Safe, Point > curve_abc_t
Definition: polynom.h:39
spline
Definition: bernstein.h:20
spline::helpers::T_Point
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
spline::polynom::polynom
polynom(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:67
spline::polynom::coeff_t_ref
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynom.h:41