polynomial.h
Go to the documentation of this file.
1 
13 #ifndef _STRUCT_POLYNOMIAL
14 #define _STRUCT_POLYNOMIAL
15 
16 #include <algorithm>
17 #include <functional>
18 #include <iostream>
19 #include <stdexcept>
20 
21 #include "MathDefs.h"
22 #include "curve_abc.h"
23 
24 namespace ndcurves {
31 template <typename Time = double, typename Numeric = Time, bool Safe = false,
32  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
33  typename T_Point =
34  std::vector<Point, Eigen::aligned_allocator<Point> > >
35 struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
36  typedef Point point_t;
37  typedef T_Point t_point_t;
38  typedef Time time_t;
39  typedef Numeric num_t;
41  typedef Eigen::MatrixXd coeff_t;
42  typedef Eigen::Ref<coeff_t> coeff_t_ref;
45 
46  /* Constructors - destructors */
47  public:
51  polynomial() : curve_abc_t(), dim_(0), degree_(0), T_min_(0), T_max_(1.0) {}
52 
59  polynomial(const coeff_t& coefficients, const time_t min, const time_t max)
60  : curve_abc_t(),
61  dim_(coefficients.rows()),
62  coefficients_(coefficients),
63  degree_(coefficients.cols() - 1),
64  T_min_(min),
65  T_max_(max) {
66  safe_check();
67  }
68 
76  polynomial(const T_Point& coefficients, const time_t min, const time_t max)
77  : curve_abc_t(),
78  dim_(coefficients.begin()->size()),
79  coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
80  degree_(coefficients_.cols() - 1),
81  T_min_(min),
82  T_max_(max) {
83  safe_check();
84  }
85 
93  template <typename In>
94  polynomial(In zeroOrderCoefficient, In out, const time_t min,
95  const time_t max)
96  : curve_abc_t(),
97  dim_(zeroOrderCoefficient->size()),
98  coefficients_(init_coeffs(zeroOrderCoefficient, out)),
99  degree_(coefficients_.cols() - 1),
100  T_min_(min),
101  T_max_(max) {
102  safe_check();
103  }
104 
112  polynomial(const Point& init, const Point& end, const time_t min,
113  const time_t max)
114  : dim_(init.size()), degree_(1), T_min_(min), T_max_(max) {
115  if (T_min_ >= T_max_)
116  throw std::invalid_argument("T_min must be strictly lower than T_max");
117  if (init.size() != end.size())
118  throw std::invalid_argument(
119  "init and end points must have the same dimensions.");
120  t_point_t coeffs;
121  coeffs.push_back(init);
122  coeffs.push_back((end - init) / (max - min));
123  coefficients_ = init_coeffs(coeffs.begin(), coeffs.end());
124  safe_check();
125  }
126 
137  polynomial(const Point& init, const Point& d_init, const Point& end,
138  const Point& d_end, const time_t min, const time_t max)
139  : dim_(init.size()), degree_(3), T_min_(min), T_max_(max) {
140  if (T_min_ >= T_max_)
141  throw std::invalid_argument("T_min must be strictly lower than T_max");
142  if (init.size() != end.size())
143  throw std::invalid_argument(
144  "init and end points must have the same dimensions.");
145  if (init.size() != d_init.size())
146  throw std::invalid_argument(
147  "init and d_init points must have the same dimensions.");
148  if (init.size() != d_end.size())
149  throw std::invalid_argument(
150  "init and d_end points must have the same dimensions.");
151  /* the coefficients [c0 c1 c2 c3] are found by solving the following system
152  of equation (found from the boundary conditions) : [1 0 0 0 ] [c0]
153  [ init ] [1 T T^2 T^3 ] x [c1] = [ end ] [0 1 0 0 ] [c2] [d_init]
154  [0 1 2T 3T^2] [c3] [d_end ]
155  */
156  double T = max - min;
157  Eigen::Matrix<double, 4, 4> m;
158  m << 1., 0, 0, 0, 1., T, T * T, T * T * T, 0, 1., 0, 0, 0, 1., 2. * T,
159  3. * T * T;
160  Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
161  Eigen::Matrix<double, 4, 1> bc; // boundary condition vector
162  coefficients_ = coeff_t::Zero(
163  dim_, degree_ + 1); // init coefficient matrix with the right size
164  for (size_t i = 0; i < dim_;
165  ++i) { // for each dimension, solve the boundary condition problem :
166  bc[0] = init[i];
167  bc[1] = end[i];
168  bc[2] = d_init[i];
169  bc[3] = d_end[i];
170  coefficients_.row(i) = (m_inv * bc).transpose();
171  }
172  safe_check();
173  }
174 
187  polynomial(const Point& init, const Point& d_init, const Point& dd_init,
188  const Point& end, const Point& d_end, const Point& dd_end,
189  const time_t min, const time_t max)
190  : dim_(init.size()), degree_(5), T_min_(min), T_max_(max) {
191  if (T_min_ >= T_max_)
192  throw std::invalid_argument("T_min must be strictly lower than T_max");
193  if (init.size() != end.size())
194  throw std::invalid_argument(
195  "init and end points must have the same dimensions.");
196  if (init.size() != d_init.size())
197  throw std::invalid_argument(
198  "init and d_init points must have the same dimensions.");
199  if (init.size() != d_end.size())
200  throw std::invalid_argument(
201  "init and d_end points must have the same dimensions.");
202  if (init.size() != dd_init.size())
203  throw std::invalid_argument(
204  "init and dd_init points must have the same dimensions.");
205  if (init.size() != dd_end.size())
206  throw std::invalid_argument(
207  "init and dd_end points must have the same dimensions.");
208  /* the coefficients [c0 c1 c2 c3 c4 c5] are found by solving the following
209  system of equation (found from the boundary conditions) : [1 0 0 0 0
210  0 ] [c0] [ init ] [1 T T^2 T^3 T^4 T^5 ] [c1] [ end ] [0
211  1 0 0 0 0 ] [c2] [d_init ] [0 1 2T 3T^2 4T^3 5T^4 ] x
212  [c3] = [d_end ] [0 0 2 0 0 0 ] [c4] [dd_init] [0 0 2 6T
213  12T^2 20T^3] [c5] [dd_end ]
214  */
215  double T = max - min;
216  Eigen::Matrix<double, 6, 6> m;
217  m << 1., 0, 0, 0, 0, 0, 1., T, T * T, pow(T, 3), pow(T, 4), pow(T, 5), 0,
218  1., 0, 0, 0, 0, 0, 1., 2. * T, 3. * T * T, 4. * pow(T, 3),
219  5. * pow(T, 4), 0, 0, 2, 0, 0, 0, 0, 0, 2, 6. * T, 12. * T * T,
220  20. * pow(T, 3);
221  Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
222  Eigen::Matrix<double, 6, 1> bc; // boundary condition vector
223  coefficients_ = coeff_t::Zero(
224  dim_, degree_ + 1); // init coefficient matrix with the right size
225  for (size_t i = 0; i < dim_;
226  ++i) { // for each dimension, solve the boundary condition problem :
227  bc[0] = init[i];
228  bc[1] = end[i];
229  bc[2] = d_init[i];
230  bc[3] = d_end[i];
231  bc[4] = dd_init[i];
232  bc[5] = dd_end[i];
233  coefficients_.row(i) = (m_inv * bc).transpose();
234  }
235  safe_check();
236  }
237 
239  virtual ~polynomial() {}
240 
241  polynomial(const polynomial& other)
242  : dim_(other.dim_),
244  degree_(other.degree_),
245  T_min_(other.T_min_),
246  T_max_(other.T_max_) {}
247 
248  // polynomial& operator=(const polynomial& other);
249 
260  static polynomial_t MinimumJerk(const point_t& p_init, const point_t& p_final,
261  const time_t t_min = 0.,
262  const time_t t_max = 1.) {
263  polynomial_t out =
264  polynomial(coeff_t::Zero(p_init.size(), 6), t_min, t_max);
265  MinimumJerk(out, p_init, p_final, t_min, t_max);
266  return out;
267  }
268 
280  static void MinimumJerk(polynomial_t& out, const point_t& p_init,
281  const point_t& p_final, const time_t t_min = 0.,
282  const time_t t_max = 1.) {
283  if (t_min > t_max)
284  throw std::invalid_argument(
285  "final time should be superior or equal to initial time.");
286  const size_t dim(p_init.size());
287  if (static_cast<size_t>(p_final.size()) != dim)
288  throw std::invalid_argument(
289  "Initial and final points must have the same dimension.");
290  const double T = t_max - t_min;
291  const double T2 = T * T;
292  const double T3 = T2 * T;
293  const double T4 = T3 * T;
294  const double T5 = T4 * T;
295 
296  assert(out.coefficients_.cols() == 6);
297  assert(out.coefficients_.rows() == static_cast<Eigen::Index>(dim));
298  assert(out.dim_ == dim);
299  out.coefficients_.fill(0.0);
300  out.coefficients_.col(0) = p_init;
301  out.coefficients_.col(3) = 10 * (p_final - p_init) / T3;
302  out.coefficients_.col(4) = -15 * (p_final - p_init) / T4;
303  out.coefficients_.col(5) = 6 * (p_final - p_init) / T5;
304  out.degree_ = 5;
305  out.T_min_ = t_min;
306  out.T_max_ = t_max;
307  out.safe_check();
308  }
309 
310  private:
311  void safe_check() {
312  if (Safe) {
313  if (T_min_ > T_max_) {
314  throw std::invalid_argument("Tmin should be inferior to Tmax");
315  }
316  if (coefficients_.cols() != int(degree_ + 1)) {
317  throw std::runtime_error("Spline order and coefficients do not match");
318  }
319  }
320  }
321 
322  /* Constructors - destructors */
323 
324  /*Operations*/
325  public:
329  virtual point_t operator()(const time_t t) const {
330  check_if_not_empty();
331  if ((t < T_min_ || t > T_max_) && Safe) {
332  throw std::invalid_argument(
333  "error in polynomial : time t to evaluate should be in range [Tmin, "
334  "Tmax] of the curve");
335  }
336  time_t const dt(t - T_min_);
337  point_t h = coefficients_.col(degree_);
338  for (int i = (int)(degree_ - 1); i >= 0; i--) {
339  h = dt * h + coefficients_.col(i);
340  }
341  return h;
342  }
343 
353  bool isApprox(
354  const polynomial_t& other,
355  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
356  return ndcurves::isApprox<num_t>(T_min_, other.min()) &&
357  ndcurves::isApprox<num_t>(T_max_, other.max()) &&
358  dim_ == other.dim() && degree_ == other.degree() &&
359  coefficients_.isApprox(other.coefficients_, prec);
360  }
361 
362  virtual bool isApprox(
363  const curve_abc_t* other,
364  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
365  const polynomial_t* other_cast = dynamic_cast<const polynomial_t*>(other);
366  if (other_cast)
367  return isApprox(*other_cast, prec);
368  else
369  return false;
370  }
371 
372  virtual bool operator==(const polynomial_t& other) const {
373  return isApprox(other);
374  }
375 
376  virtual bool operator!=(const polynomial_t& other) const {
377  return !(*this == other);
378  }
379 
385  virtual point_t derivate(const time_t t, const std::size_t order) const {
386  check_if_not_empty();
387  if ((t < T_min_ || t > T_max_) && Safe) {
388  throw std::invalid_argument(
389  "error in polynomial : time t to evaluate derivative should be in "
390  "range [Tmin, Tmax] of the curve");
391  }
392  time_t const dt(t - T_min_);
393  time_t cdt(1);
394  point_t currentPoint_ = point_t::Zero(dim_);
395  for (int i = (int)(order); i < (int)(degree_ + 1); ++i, cdt *= dt) {
396  currentPoint_ += cdt * coefficients_.col(i) * fact(i, order);
397  }
398  return currentPoint_;
399  }
400 
401  polynomial_t compute_derivate(const std::size_t order) const {
402  check_if_not_empty();
403  if (order == 0) {
404  return *this;
405  }
406  coeff_t coeff_derivated = deriv_coeff(coefficients_);
407  polynomial_t deriv(coeff_derivated, T_min_, T_max_);
408  return deriv.compute_derivate(order - 1);
409  }
410 
415  polynomial_t* compute_derivate_ptr(const std::size_t order) const {
416  return new polynomial_t(compute_derivate(order));
417  }
418 
419  Eigen::MatrixXd coeff() const { return coefficients_; }
420 
421  point_t coeffAtDegree(const std::size_t degree) const {
422  point_t res;
423  if (degree <= degree_) {
424  res = coefficients_.col(degree);
425  }
426  return res;
427  }
428 
429  private:
430  num_t fact(const std::size_t n, const std::size_t order) const {
431  num_t res(1);
432  for (std::size_t i = 0; i < std::size_t(order); ++i) {
433  res *= (num_t)(n - i);
434  }
435  return res;
436  }
437 
438  coeff_t deriv_coeff(coeff_t coeff) const {
439  if (coeff.cols() == 1) // only the constant part is left, fill with 0
440  return coeff_t::Zero(coeff.rows(), 1);
441  coeff_t coeff_derivated(coeff.rows(), coeff.cols() - 1);
442  for (std::size_t i = 0; i < std::size_t(coeff_derivated.cols()); i++) {
443  coeff_derivated.col(i) = coeff.col(i + 1) * (num_t)(i + 1);
444  }
445  return coeff_derivated;
446  }
447 
448  void check_if_not_empty() const {
449  if (coefficients_.size() == 0) {
450  throw std::runtime_error(
451  "Error in polynomial : there is no coefficients set / did you use "
452  "empty constructor ?");
453  }
454  }
455  /*Operations*/
456 
457  public:
458  /*Helpers*/
461  std::size_t virtual dim() const { return dim_; };
464  num_t virtual min() const { return T_min_; }
467  num_t virtual max() const { return T_max_; }
470  virtual std::size_t degree() const { return degree_; }
471  /*Helpers*/
472 
473  polynomial_t& operator+=(const polynomial_t& p1) {
474  assert_operator_compatible(p1);
475  if (p1.degree() > degree()) {
476  polynomial_t::coeff_t res = p1.coeff();
477  res.block(0, 0, coefficients_.rows(), coefficients_.cols()) +=
479  coefficients_ = res;
480  degree_ = p1.degree();
481  } else {
482  coefficients_.block(0, 0, p1.coeff().rows(), p1.coeff().cols()) +=
483  p1.coeff();
484  }
485  return *this;
486  }
487 
488  polynomial_t& operator-=(const polynomial_t& p1) {
489  assert_operator_compatible(p1);
490  if (p1.degree() > degree()) {
491  polynomial_t::coeff_t res = -p1.coeff();
492  res.block(0, 0, coefficients_.rows(), coefficients_.cols()) +=
494  coefficients_ = res;
495  degree_ = p1.degree();
496  } else {
497  coefficients_.block(0, 0, p1.coeff().rows(), p1.coeff().cols()) -=
498  p1.coeff();
499  }
500  return *this;
501  }
502 
503  polynomial_t& operator+=(const polynomial_t::point_t& point) {
504  coefficients_.col(0) += point;
505  return *this;
506  }
507 
508  polynomial_t& operator-=(const polynomial_t::point_t& point) {
509  coefficients_.col(0) -= point;
510  return *this;
511  }
512 
513  polynomial_t& operator/=(const double d) {
514  coefficients_ /= d;
515  return *this;
516  }
517 
518  polynomial_t& operator*=(const double d) {
519  coefficients_ *= d;
520  return *this;
521  }
522 
531  polynomial_t cross(const polynomial_t& pOther) const {
532  assert_operator_compatible(pOther);
533  if (dim() != 3)
534  throw std::invalid_argument(
535  "Can't perform cross product on polynomials with dimensions != 3 ");
536  std::size_t new_degree = degree() + pOther.degree();
537  coeff_t nCoeffs = Eigen::MatrixXd::Zero(3, new_degree + 1);
538  Eigen::Vector3d currentVec;
539  Eigen::Vector3d currentVecCrossed;
540  for (long i = 0; i < coefficients_.cols(); ++i) {
541  currentVec = coefficients_.col(i);
542  for (long j = 0; j < pOther.coeff().cols(); ++j) {
543  currentVecCrossed = pOther.coeff().col(j);
544  nCoeffs.col(i + j) += currentVec.cross(currentVecCrossed);
545  }
546  }
547  // remove last degrees is they are equal to 0
548  long final_degree = new_degree;
549  while (nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN &&
550  final_degree > 0) {
551  --final_degree;
552  }
553  return polynomial_t(nCoeffs.leftCols(final_degree + 1), min(), max());
554  }
555 
564  polynomial_t cross(const polynomial_t::point_t& point) const {
565  if (dim() != 3)
566  throw std::invalid_argument(
567  "Can't perform cross product on polynomials with dimensions != 3 ");
568  coeff_t nCoeffs = coefficients_;
569  Eigen::Vector3d currentVec;
570  Eigen::Vector3d pointVec = point;
571  for (long i = 0; i < coefficients_.cols(); ++i) {
572  currentVec = coefficients_.col(i);
573  nCoeffs.col(i) = currentVec.cross(pointVec);
574  }
575  // remove last degrees is they are equal to 0
576  long final_degree = degree();
577  while (nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN &&
578  final_degree > 0) {
579  --final_degree;
580  }
581  return polynomial_t(nCoeffs.leftCols(final_degree + 1), min(), max());
582  }
583 
584  /*Attributes*/
585  std::size_t dim_; // const
586  coeff_t coefficients_; // const
587  std::size_t degree_; // const
588  time_t T_min_, T_max_; // const
589  /*Attributes*/
590 
591  private:
592  void assert_operator_compatible(const polynomial_t& other) const {
593  if ((fabs(min() - other.min()) > ndcurves::MARGIN) ||
594  (fabs(max() - other.max()) > ndcurves::MARGIN) ||
595  dim() != other.dim()) {
596  throw std::invalid_argument(
597  "Can't perform base operation (+ - ) on two polynomials with "
598  "different time ranges or different dimensions");
599  }
600  }
601 
602  template <typename In>
603  coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
604  std::size_t size =
605  std::distance(zeroOrderCoefficient, highestOrderCoefficient);
606  coeff_t res = coeff_t(dim_, size);
607  int i = 0;
608  for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient;
609  ++cit, ++i) {
610  res.col(i) = *cit;
611  }
612  return res;
613  }
614 
615  public:
616  // Serialization of the class
618 
619  template <class Archive>
620  void serialize(Archive& ar, const unsigned int version) {
621  if (version) {
622  // Do something depending on version ?
623  }
624  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
625  ar& boost::serialization::make_nvp("dim", dim_);
626  ar& boost::serialization::make_nvp("coefficients", coefficients_);
627  ar& boost::serialization::make_nvp("dim", dim_);
628  ar& boost::serialization::make_nvp("degree", degree_);
629  ar& boost::serialization::make_nvp("T_min", T_min_);
630  ar& boost::serialization::make_nvp("T_max", T_max_);
631  }
632 
633 }; // class polynomial
634 
635 template <typename T, typename N, bool S, typename P, typename TP>
637  const polynomial<T, N, S, P, TP>& p2) {
639  return res += p2;
640 }
641 
642 template <typename T, typename N, bool S, typename P, typename TP>
644  const polynomial<T, N, S, P, TP>& p1,
645  const typename polynomial<T, N, S, P, TP>::point_t& point) {
647  return res += point;
648 }
649 
650 template <typename T, typename N, bool S, typename P, typename TP>
652  const typename polynomial<T, N, S, P, TP>::point_t& point,
653  const polynomial<T, N, S, P, TP>& p1) {
655  return res += point;
656 }
657 
658 template <typename T, typename N, bool S, typename P, typename TP>
660  const polynomial<T, N, S, P, TP>& p1,
661  const typename polynomial<T, N, S, P, TP>::point_t& point) {
663  return res -= point;
664 }
665 
666 template <typename T, typename N, bool S, typename P, typename TP>
668  const typename polynomial<T, N, S, P, TP>::point_t& point,
669  const polynomial<T, N, S, P, TP>& p1) {
671  return res += point;
672 }
673 
674 template <typename T, typename N, bool S, typename P, typename TP>
676  typename polynomial<T, N, S, P, TP>::coeff_t res = -p1.coeff();
677  return polynomial<T, N, S, P, TP>(res, p1.min(), p1.max());
678 }
679 
680 template <typename T, typename N, bool S, typename P, typename TP>
682  const polynomial<T, N, S, P, TP>& p2) {
684  return res -= p2;
685 }
686 
687 template <typename T, typename N, bool S, typename P, typename TP>
689  const double k) {
691  return res /= k;
692 }
693 
694 template <typename T, typename N, bool S, typename P, typename TP>
696  const double k) {
698  return res *= k;
699 }
700 
701 template <typename T, typename N, bool S, typename P, typename TP>
703  const polynomial<T, N, S, P, TP>& p1) {
705  return res *= k;
706 }
707 
708 } // namespace ndcurves
709 
711  SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
712  typename T_Point),
714 
715 #endif //_STRUCT_POLYNOMIAL
#define SINGLE_ARG(...)
Definition: archive.hpp:23
Definition: bernstein.h:20
polynomial< Time, Numeric, Safe, Point, T_Point > polynomial_t
Definition: polynomial.h:43
polynomial_t & operator*=(const double d)
Definition: polynomial.h:518
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: polynomial.h:464
polynomial_t & operator+=(const polynomial_t::point_t &point)
Definition: polynomial.h:503
time_t T_max_
Definition: polynomial.h:588
coeff_t coefficients_
Definition: polynomial.h:586
std::size_t degree_
Definition: polynomial.h:587
polynomial(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:59
static void MinimumJerk(polynomial_t &out, const point_t &p_init, const point_t &p_final, const time_t t_min=0., const time_t t_max=1.)
MinimumJerk Build a polynomial curve connecting p_init to p_final minimizing the time integral of the...
Definition: polynomial.h:280
interface for a Curve of arbitrary dimension.
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner&#39;s scheme.
Definition: polynomial.h:329
polynomial(const polynomial &other)
Definition: polynomial.h:241
polynomial_t cross(const polynomial_t &pOther) const
Compute the cross product of the current polynomial by another polynomial. The cross product p1Xp2 of...
Definition: polynomial.h:531
friend class boost::serialization::access
Definition: polynomial.h:617
virtual bool operator==(const polynomial_t &other) const
Definition: polynomial.h:372
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:805
polynomial(const Point &init, const Point &end, const time_t min, const time_t max)
Constructor from boundary condition with C0 : create a polynomial that connect exactly init and end (...
Definition: polynomial.h:112
bezier_curve< T, N, S, P > operator+(const bezier_curve< T, N, S, P > &p1, const bezier_curve< T, N, S, P > &p2)
Definition: bezier_curve.h:748
point_t coeffAtDegree(const std::size_t degree) const
Definition: polynomial.h:421
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: polynomial.h:44
Numeric num_t
Definition: polynomial.h:39
T_Point t_point_t
Definition: polynomial.h:37
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:46
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: polynomial.h:40
virtual bool operator!=(const polynomial_t &other) const
Definition: polynomial.h:376
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: fwd.h:42
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:812
double Time
Definition: effector_spline.h:27
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynomial.h:42
polynomial(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:76
polynomial_t & operator/=(const double d)
Definition: polynomial.h:513
virtual std::size_t degree() const
Get the degree of the curve.
Definition: polynomial.h:470
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: polynomial.h:467
virtual std::size_t dim() const
Get dimension of curve.
Definition: polynomial.h:461
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
Time time_t
Definition: polynomial.h:38
polynomial_t cross(const polynomial_t::point_t &point) const
Compute the cross product of the current polynomial p by a point point. The cross product pXpoint of ...
Definition: polynomial.h:564
void serialize(Archive &ar, const unsigned int version)
Definition: polynomial.h:620
bool isApprox(const polynomial_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: polynomial.h:353
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
polynomial()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: polynomial.h:51
Point point_t
Definition: polynomial.h:36
polynomial(const Point &init, const Point &d_init, const Point &end, const Point &d_end, const time_t min, const time_t max)
Constructor from boundary condition with C1 : create a polynomial that connect exactly init and end a...
Definition: polynomial.h:137
double Numeric
Definition: effector_spline.h:26
polynomial_t & operator+=(const polynomial_t &p1)
Definition: polynomial.h:473
Eigen::MatrixXd coeff() const
Definition: polynomial.h:419
polynomial_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: polynomial.h:415
polynomial_t & operator-=(const polynomial_t::point_t &point)
Definition: polynomial.h:508
time_t T_min_
Definition: polynomial.h:588
polynomial(const Point &init, const Point &d_init, const Point &dd_init, const Point &end, const Point &d_end, const Point &dd_end, const time_t min, const time_t max)
Constructor from boundary condition with C2 : create a polynomial that connect exactly init and end a...
Definition: polynomial.h:187
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: polynomial.h:362
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:755
std::size_t dim_
Definition: polynomial.h:585
polynomial_t compute_derivate(const std::size_t order) const
Definition: polynomial.h:401
static polynomial_t MinimumJerk(const point_t &p_init, const point_t &p_final, const time_t t_min=0., const time_t t_max=1.)
MinimumJerk Build a polynomial curve connecting p_init to p_final minimizing the time integral of the...
Definition: polynomial.h:260
virtual ~polynomial()
Destructor.
Definition: polynomial.h:239
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: polynomial.h:385
Eigen::MatrixXd coeff_t
Definition: polynomial.h:41
polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:94
polynomial_t & operator-=(const polynomial_t &p1)
Definition: polynomial.h:488