13 #ifndef _STRUCT_POLYNOMIAL 14 #define _STRUCT_POLYNOMIAL 32 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
33 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
34 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> > >
35 struct polynomial :
public curve_abc<Time, Numeric, Safe, Point> {
60 dim_(coefficients.rows()),
62 degree_(coefficients.cols() - 1),
76 dim_(coefficients.begin()->size()),
77 coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
91 template <
typename In>
94 dim_(zeroOrderCoefficient->size()),
109 if (
T_min_ >=
T_max_)
throw std::invalid_argument(
"T_min must be strictly lower than T_max");
110 if (init.size() != end.size())
throw std::invalid_argument(
"init and end points must have the same dimensions.");
112 coeffs.push_back(init);
113 coeffs.push_back((end - init) / (max - min));
131 if (
T_min_ >=
T_max_)
throw std::invalid_argument(
"T_min must be strictly lower than T_max");
132 if (init.size() != end.size())
throw std::invalid_argument(
"init and end points must have the same dimensions.");
133 if (init.size() != d_init.size())
134 throw std::invalid_argument(
"init and d_init points must have the same dimensions.");
135 if (init.size() != d_end.size())
136 throw std::invalid_argument(
"init and d_end points must have the same dimensions.");
144 double T = max -
min;
145 Eigen::Matrix<double, 4, 4> m;
146 m << 1., 0, 0, 0, 1., T, T * T, T * T * T, 0, 1., 0, 0, 0, 1., 2. * T, 3. * T * T;
147 Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
148 Eigen::Matrix<double, 4, 1> bc;
150 for (
size_t i = 0; i <
dim_; ++i) {
173 const Point& dd_end,
const time_t
min,
const time_t
max)
175 if (
T_min_ >=
T_max_)
throw std::invalid_argument(
"T_min must be strictly lower than T_max");
176 if (init.size() != end.size())
throw std::invalid_argument(
"init and end points must have the same dimensions.");
177 if (init.size() != d_init.size())
178 throw std::invalid_argument(
"init and d_init points must have the same dimensions.");
179 if (init.size() != d_end.size())
180 throw std::invalid_argument(
"init and d_end points must have the same dimensions.");
181 if (init.size() != dd_init.size())
182 throw std::invalid_argument(
"init and dd_init points must have the same dimensions.");
183 if (init.size() != dd_end.size())
184 throw std::invalid_argument(
"init and dd_end points must have the same dimensions.");
194 double T = max -
min;
195 Eigen::Matrix<double, 6, 6> m;
196 m << 1., 0, 0, 0, 0, 0, 1., T, T * T, pow(T, 3), pow(T, 4), pow(T, 5), 0, 1., 0, 0, 0, 0, 0, 1., 2. * T,
197 3. * T * T, 4. * pow(T, 3), 5. * pow(T, 4), 0, 0, 2, 0, 0, 0, 0, 0, 2, 6. * T, 12. * T * T, 20. * pow(T, 3);
198 Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
199 Eigen::Matrix<double, 6, 1> bc;
201 for (
size_t i = 0; i <
dim_; ++i) {
234 static polynomial_t
MinimumJerk(
const point_t& p_init,
const point_t& p_final,
const time_t t_min = 0.,
235 const time_t t_max = 1.) {
236 if (t_min > t_max)
throw std::invalid_argument(
"final time should be superior or equal to initial time.");
237 const size_t dim(p_init.size());
238 if (static_cast<size_t>(p_final.size()) !=
dim)
239 throw std::invalid_argument(
"Initial and final points must have the same dimension.");
240 const double T = t_max - t_min;
241 const double T2 = T * T;
242 const double T3 = T2 * T;
243 const double T4 = T3 * T;
244 const double T5 = T4 * T;
246 coeff_t coeffs = coeff_t::Zero(
dim, 6);
247 coeffs.col(0) = p_init;
248 coeffs.col(3) = 10 * (p_final - p_init) / T3;
249 coeffs.col(4) = -15 * (p_final - p_init) / T4;
250 coeffs.col(5) = 6 * (p_final - p_init) / T5;
258 throw std::invalid_argument(
"Tmin should be inferior to Tmax");
261 throw std::runtime_error(
"Spline order and coefficients do not match");
274 check_if_not_empty();
275 if ((t < T_min_ || t >
T_max_) && Safe) {
276 throw std::invalid_argument(
277 "error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
279 time_t
const dt(t -
T_min_);
281 for (
int i = (
int)(
degree_ - 1); i >= 0; i--) {
295 bool isApprox(
const polynomial_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
296 return ndcurves::isApprox<num_t>(
T_min_, other.
min()) && ndcurves::isApprox<num_t>(
T_max_, other.
max()) &&
301 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
302 const polynomial_t* other_cast =
dynamic_cast<const polynomial_t*
>(other);
311 virtual bool operator!=(
const polynomial_t& other)
const {
return !(*
this == other); }
317 virtual point_t
derivate(
const time_t t,
const std::size_t order)
const {
318 check_if_not_empty();
319 if ((t < T_min_ || t >
T_max_) && Safe) {
320 throw std::invalid_argument(
321 "error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
323 time_t
const dt(t -
T_min_);
325 point_t currentPoint_ = point_t::Zero(
dim_);
326 for (
int i = (
int)(order); i < (int)(
degree_ + 1); ++i, cdt *= dt) {
327 currentPoint_ += cdt *
coefficients_.col(i) * fact(i, order);
329 return currentPoint_;
333 check_if_not_empty();
360 num_t fact(
const std::size_t n,
const std::size_t order)
const {
362 for (std::size_t i = 0; i < std::size_t(order); ++i) {
363 res *= (
num_t)(n - i);
368 coeff_t deriv_coeff(coeff_t
coeff)
const {
369 if (coeff.cols() == 1)
370 return coeff_t::Zero(coeff.rows(), 1);
371 coeff_t coeff_derivated(coeff.rows(), coeff.cols() - 1);
372 for (std::size_t i = 0; i < std::size_t(coeff_derivated.cols()); i++) {
373 coeff_derivated.col(i) = coeff.col(i + 1) * (
num_t)(i + 1);
375 return coeff_derivated;
378 void check_if_not_empty()
const {
380 throw std::runtime_error(
"Error in polynomial : there is no coefficients set / did you use empty constructor ?");
389 std::size_t
virtual dim()
const {
return dim_; };
402 assert_operator_compatible(p1);
416 assert_operator_compatible(p1);
455 polynomial_t
cross(
const polynomial_t& pOther)
const {
456 assert_operator_compatible(pOther);
458 throw std::invalid_argument(
"Can't perform cross product on polynomials with dimensions != 3 ");
460 coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,new_degree+1);
461 Eigen::Vector3d currentVec;
462 Eigen::Vector3d currentVecCrossed;
465 for(
long j = 0; j< pOther.
coeff().cols(); ++j){
466 currentVecCrossed = pOther.
coeff().col(j);
467 nCoeffs.col(i+j) += currentVec.cross(currentVecCrossed);
471 long final_degree = new_degree;
472 while(nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN && final_degree >0){
486 throw std::invalid_argument(
"Can't perform cross product on polynomials with dimensions != 3 ");
488 Eigen::Vector3d currentVec;
489 Eigen::Vector3d pointVec = point;
492 nCoeffs.col(i) = currentVec.cross(pointVec);
495 long final_degree =
degree();
496 while(nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN && final_degree >0){
511 void assert_operator_compatible(
const polynomial_t& other)
const{
512 if ((fabs(
min() - other.
min()) > ndcurves::MARGIN) || (fabs(
max() - other.
max()) > ndcurves::MARGIN) ||
dim() != other.
dim()){
513 throw std::invalid_argument(
"Can't perform base operation (+ - ) on two polynomials with different time ranges or different dimensions");
517 template <
typename In>
518 coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
519 std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
520 coeff_t res =
coeff_t(dim_, size);
522 for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i) {
532 template <
class Archive>
533 void serialize(Archive& ar,
const unsigned int version) {
537 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
538 ar& boost::serialization::make_nvp(
"dim", dim_);
539 ar& boost::serialization::make_nvp(
"coefficients", coefficients_);
540 ar& boost::serialization::make_nvp(
"dim", dim_);
541 ar& boost::serialization::make_nvp(
"degree", degree_);
542 ar& boost::serialization::make_nvp(
"T_min", T_min_);
543 ar& boost::serialization::make_nvp(
"T_max", T_max_);
548 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
549 polynomial<T,N,S,P,TP> operator+(
const polynomial<T,N,S,P,TP>& p1,
const polynomial<T,N,S,P,TP>& p2) {
554 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
555 polynomial<T,N,S,P,TP> operator+(
const polynomial<T,N,S,P,TP>& p1,
const typename polynomial<T,N,S,P,TP>::point_t& point) {
560 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
561 polynomial<T,N,S,P,TP> operator+(
const typename polynomial<T,N,S,P,TP>::point_t& point,
const polynomial<T,N,S,P,TP>& p1) {
566 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
567 polynomial<T,N,S,P,TP> operator-(
const polynomial<T,N,S,P,TP>& p1,
const typename polynomial<T,N,S,P,TP>::point_t& point) {
572 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
573 polynomial<T,N,S,P,TP> operator-(
const typename polynomial<T,N,S,P,TP>::point_t& point,
const polynomial<T,N,S,P,TP>& p1) {
579 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
585 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
586 polynomial<T,N,S,P,TP> operator-(
const polynomial<T,N,S,P,TP>& p1,
const polynomial<T,N,S,P,TP>& p2) {
591 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
597 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
603 template <
typename T,
typename N,
bool S,
typename P,
typename TP >
611 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point,
typename T_Point),
613 #endif //_STRUCT_POLYNOMIAL 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:444
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: polynomial.h:392
polynomial_t & operator+=(const polynomial_t::point_t &point)
Definition: polynomial.h:429
time_t T_max_
Definition: polynomial.h:506
coeff_t coefficients_
Definition: polynomial.h:504
std::size_t degree_
Definition: polynomial.h:505
polynomial(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:58
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's scheme.
Definition: polynomial.h:273
polynomial(const polynomial &other)
Definition: polynomial.h:216
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:677
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:455
friend class boost::serialization::access
Definition: polynomial.h:530
virtual bool operator==(const polynomial_t &other) const
Definition: polynomial.h:309
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:107
point_t coeffAtDegree(const std::size_t degree) const
Definition: polynomial.h:351
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:41
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: polynomial.h:40
virtual bool operator!=(const polynomial_t &other) const
Definition: polynomial.h:311
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:37
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:74
polynomial_t & operator/=(const double d)
Definition: polynomial.h:439
virtual std::size_t degree() const
Get the degree of the curve.
Definition: polynomial.h:398
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: polynomial.h:395
virtual std::size_t dim() const
Get dimension of curve.
Definition: polynomial.h:389
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:484
void serialize(Archive &ar, const unsigned int version)
Definition: polynomial.h:533
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:295
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:50
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:128
double Numeric
Definition: effector_spline.h:26
polynomial_t & operator+=(const polynomial_t &p1)
Definition: polynomial.h:401
Eigen::MatrixXd coeff() const
Definition: polynomial.h:349
polynomial_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: polynomial.h:345
polynomial_t & operator-=(const polynomial_t::point_t &point)
Definition: polynomial.h:434
time_t T_min_
Definition: polynomial.h:506
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:719
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:725
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:172
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: polynomial.h:300
std::size_t dim_
Definition: polynomial.h:503
polynomial_t compute_derivate(const std::size_t order) const
Definition: polynomial.h:332
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:234
virtual ~polynomial()
Destructor.
Definition: polynomial.h:214
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:317
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:92
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:671
polynomial_t & operator-=(const polynomial_t &p1)
Definition: polynomial.h:415