13#ifndef _STRUCT_POLYNOMIAL
14#define _STRUCT_POLYNOMIAL
31template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
32 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
34 std::vector<Point, Eigen::aligned_allocator<Point> > >
61 dim_(coefficients.rows()),
63 degree_(coefficients.cols() - 1),
78 dim_(coefficients.begin()->size()),
79 coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
93 template <
typename In>
97 dim_(zeroOrderCoefficient->size()),
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.");
121 coeffs.push_back(init);
122 coeffs.push_back((end - init) / (
max -
min));
137 polynomial(
const Point& init,
const Point& d_init,
const Point& end,
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.");
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,
160 Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
161 Eigen::Matrix<double, 4, 1> bc;
164 for (
size_t i = 0; i <
dim_;
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,
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.");
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,
221 Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
222 Eigen::Matrix<double, 6, 1> bc;
225 for (
size_t i = 0; i <
dim_;
262 const time_t t_max = 1.) {
264 throw std::invalid_argument(
265 "final time should be superior or equal to initial time.");
266 const size_t dim(p_init.size());
267 if (
static_cast<size_t>(p_final.size()) !=
dim)
268 throw std::invalid_argument(
269 "Initial and final points must have the same dimension.");
270 const double T = t_max - t_min;
271 const double T2 = T * T;
272 const double T3 = T2 * T;
273 const double T4 = T3 * T;
274 const double T5 = T4 * T;
277 coeff_t::Zero(
dim, 6);
278 coeffs.col(0) = p_init;
279 coeffs.col(3) = 10 * (p_final - p_init) / T3;
280 coeffs.col(4) = -15 * (p_final - p_init) / T4;
281 coeffs.col(5) = 6 * (p_final - p_init) / T5;
289 throw std::invalid_argument(
"Tmin should be inferior to Tmax");
292 throw std::runtime_error(
"Spline order and coefficients do not match");
305 check_if_not_empty();
306 if ((t < T_min_ || t >
T_max_) && Safe) {
307 throw std::invalid_argument(
308 "error in polynomial : time t to evaluate should be in range [Tmin, "
309 "Tmax] of the curve");
313 for (
int i = (
int)(
degree_ - 1); i >= 0; i--) {
330 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
331 return ndcurves::isApprox<num_t>(
T_min_, other.
min()) &&
332 ndcurves::isApprox<num_t>(
T_max_, other.
max()) &&
339 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
352 return !(*
this == other);
361 check_if_not_empty();
362 if ((t < T_min_ || t >
T_max_) && Safe) {
363 throw std::invalid_argument(
364 "error in polynomial : time t to evaluate derivative should be in "
365 "range [Tmin, Tmax] of the curve");
370 for (
int i = (
int)(order); i < (int)(
degree_ + 1); ++i, cdt *= dt) {
371 currentPoint_ += cdt *
coefficients_.col(i) * fact(i, order);
373 return currentPoint_;
377 check_if_not_empty();
405 num_t fact(
const std::size_t n,
const std::size_t order)
const {
407 for (std::size_t i = 0; i < std::size_t(order); ++i) {
408 res *= (
num_t)(n - i);
414 if (
coeff.cols() == 1)
415 return coeff_t::Zero(
coeff.rows(), 1);
417 for (std::size_t i = 0; i < std::size_t(coeff_derivated.cols()); i++) {
418 coeff_derivated.col(i) =
coeff.col(i + 1) * (
num_t)(i + 1);
420 return coeff_derivated;
423 void check_if_not_empty()
const {
425 throw std::runtime_error(
426 "Error in polynomial : there is no coefficients set / did you use "
427 "empty constructor ?");
436 std::size_t
virtual dim()
const {
return dim_; };
449 assert_operator_compatible(p1);
464 assert_operator_compatible(p1);
507 assert_operator_compatible(pOther);
509 throw std::invalid_argument(
510 "Can't perform cross product on polynomials with dimensions != 3 ");
512 coeff_t nCoeffs = Eigen::MatrixXd::Zero(3, new_degree + 1);
513 Eigen::Vector3d currentVec;
514 Eigen::Vector3d currentVecCrossed;
517 for (
long j = 0; j < pOther.
coeff().cols(); ++j) {
518 currentVecCrossed = pOther.
coeff().col(j);
519 nCoeffs.col(i + j) += currentVec.cross(currentVecCrossed);
523 long final_degree = new_degree;
524 while (nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN &&
541 throw std::invalid_argument(
542 "Can't perform cross product on polynomials with dimensions != 3 ");
544 Eigen::Vector3d currentVec;
545 Eigen::Vector3d pointVec = point;
548 nCoeffs.col(i) = currentVec.cross(pointVec);
551 long final_degree =
degree();
552 while (nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN &&
567 void assert_operator_compatible(
const polynomial_t& other)
const {
568 if ((fabs(
min() - other.
min()) > ndcurves::MARGIN) ||
569 (fabs(
max() - other.
max()) > ndcurves::MARGIN) ||
571 throw std::invalid_argument(
572 "Can't perform base operation (+ - ) on two polynomials with "
573 "different time ranges or different dimensions");
577 template <
typename In>
578 coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
580 std::distance(zeroOrderCoefficient, highestOrderCoefficient);
583 for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient;
594 template <
class Archive>
595 void serialize(Archive& ar,
const unsigned int version) {
599 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
curve_abc_t);
600 ar& boost::serialization::make_nvp(
"dim",
dim_);
601 ar& boost::serialization::make_nvp(
"coefficients",
coefficients_);
602 ar& boost::serialization::make_nvp(
"dim",
dim_);
603 ar& boost::serialization::make_nvp(
"degree",
degree_);
604 ar& boost::serialization::make_nvp(
"T_min",
T_min_);
605 ar& boost::serialization::make_nvp(
"T_max",
T_max_);
610template <
typename T,
typename N,
bool S,
typename P,
typename TP>
617template <
typename T,
typename N,
bool S,
typename P,
typename TP>
625template <
typename T,
typename N,
bool S,
typename P,
typename TP>
633template <
typename T,
typename N,
bool S,
typename P,
typename TP>
641template <
typename T,
typename N,
bool S,
typename P,
typename TP>
649template <
typename T,
typename N,
bool S,
typename P,
typename TP>
655template <
typename T,
typename N,
bool S,
typename P,
typename TP>
662template <
typename T,
typename N,
bool S,
typename P,
typename TP>
669template <
typename T,
typename N,
bool S,
typename P,
typename TP>
676template <
typename T,
typename N,
bool S,
typename P,
typename TP>
686 SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point,
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
interface for a Curve of arbitrary dimension.
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
Definition: bernstein.h:20
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:812
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:805
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:755
polynomial_t::coeff_t coeff_t
Definition: python_definitions.h:32
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
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:46
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: polynomial.h:35
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: polynomial.h:439
polynomial< Time, Numeric, Safe, Point, T_Point > polynomial_t
Definition: polynomial.h:43
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:506
polynomial()
Empty constructor. Curve obtained this way can not perform other class functions.
Definition: polynomial.h:51
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:539
virtual std::size_t dim() const
Get dimension of curve.
Definition: polynomial.h:436
polynomial_t compute_derivate(const std::size_t order) const
Definition: polynomial.h:376
virtual bool operator!=(const polynomial_t &other) const
Definition: polynomial.h:351
Time time_t
Definition: polynomial.h:38
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: polynomial.h:40
polynomial_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: polynomial.h:390
std::size_t dim_
Definition: polynomial.h:560
Point point_t
Definition: polynomial.h:36
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:328
Numeric num_t
Definition: polynomial.h:39
void serialize(Archive &ar, const unsigned int version)
Definition: polynomial.h:595
coeff_t coefficients_
Definition: polynomial.h:561
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: polynomial.h:337
virtual ~polynomial()
Destructor.
Definition: polynomial.h:239
polynomial(const polynomial &other)
Definition: polynomial.h:241
Eigen::MatrixXd coeff() const
Definition: polynomial.h:394
polynomial_t & operator-=(const polynomial_t &p1)
Definition: polynomial.h:463
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
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:360
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
T_Point t_point_t
Definition: polynomial.h:37
polynomial_t & operator*=(const double d)
Definition: polynomial.h:493
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner's scheme.
Definition: polynomial.h:304
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: polynomial.h:44
polynomial_t & operator-=(const polynomial_t::point_t &point)
Definition: polynomial.h:483
std::size_t degree_
Definition: polynomial.h:562
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynomial.h:42
polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:94
time_t T_min_
Definition: polynomial.h:563
polynomial(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:59
polynomial_t & operator+=(const polynomial_t::point_t &point)
Definition: polynomial.h:478
polynomial(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:76
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: polynomial.h:442
virtual bool operator==(const polynomial_t &other) const
Definition: polynomial.h:347
friend class boost::serialization::access
Definition: polynomial.h:592
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
polynomial_t & operator+=(const polynomial_t &p1)
Definition: polynomial.h:448
point_t coeffAtDegree(const std::size_t degree) const
Definition: polynomial.h:396
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 std::size_t degree() const
Get the degree of the curve.
Definition: polynomial.h:445
time_t T_max_
Definition: polynomial.h:563
polynomial_t & operator/=(const double d)
Definition: polynomial.h:488
Eigen::MatrixXd coeff_t
Definition: polynomial.h:41