30#ifndef HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
31#define HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
33#include <hpp/constraints/differentiable-function.hh>
41namespace timeParameterization {
51 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor>
53 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1>
Vector_t;
62 : parameters_(parameters), breakpoints_(breakpoints) {
63 assert(breakpoints_.size() == parameters_.cols() + 1);
64 assert(parameters_.rows() == NbCoeffs);
66 for (
size_type j = 0; j < parameters_.cols(); ++j) {
68 assert(breakpoints_[j] > breakpoints_[j - 1]);
70 for (
size_type i = 0; i < parameters_.rows(); ++i) {
71 assert(parameters_(i, j) < std::numeric_limits<value_type>::infinity());
72 assert(parameters_(i, j) >
73 -std::numeric_limits<value_type>::infinity());
94 const size_t seg_index = findPolynomialIndex(t);
95 const auto& poly_coeffs = parameters_.col(seg_index);
98 for (
size_type i = 1; i < poly_coeffs.size(); ++i) {
100 res += poly_coeffs[i] * tn;
106 value_type Jac(
const value_type& t)
const {
return Jac(t, 1); }
108 value_type Jac(
const value_type& t,
const size_type& order)
const {
109 if (order >= parameters_.rows())
return 0;
110 const size_type MaxOrder = 10;
111 if (parameters_.rows() > MaxOrder)
112 throw std::invalid_argument(
113 "Cannot compute the derivative of order greater than 10.");
114 typedef path::binomials<MaxOrder> Binomials_t;
115 const Binomials_t::Factorials_t& factors = Binomials_t::factorials();
117 const size_t seg_index = findPolynomialIndex(t);
118 const auto& poly_coeffs = parameters_.col(seg_index);
122 for (size_type i = order; i < poly_coeffs.size(); ++i) {
123 res += value_type(factors[i] / factors[i - order]) * poly_coeffs[i] * tn;
129 size_t findPolynomialIndex(
const value_type& t)
const {
130 size_t seg_index = std::numeric_limits<size_t>::max();
131 for (
int i = 0; i < parameters_.size(); ++i) {
132 if (breakpoints_[i] <= t && t <= breakpoints_[i + 1]) {
137 if (seg_index == std::numeric_limits<size_t>::max()) {
138 std::ostringstream oss;
139 oss <<
"Position " << t <<
" is outside of range [ " << breakpoints_[0]
140 <<
", " << breakpoints_[breakpoints_.size() - 1] <<
']';
141 throw std::runtime_error(oss.str());
149 ParameterMatrix_t parameters_;
150 Vector_t breakpoints_;
Definition: time-parameterization.hh:37
Definition: piecewise-polynomial.hh:44
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > Vector_t
Definition: piecewise-polynomial.hh:53
value_type value(const value_type &t) const
Computes .
Definition: piecewise-polynomial.hh:85
const ParameterMatrix_t & parameters() const
Definition: piecewise-polynomial.hh:78
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor > ParameterMatrix_t
Definition: piecewise-polynomial.hh:52
PiecewisePolynomial(const ParameterMatrix_t ¶meters, const Vector_t &breakpoints)
Definition: piecewise-polynomial.hh:60
TimeParameterizationPtr_t copy() const
Definition: piecewise-polynomial.hh:80
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition: piecewise-polynomial.hh:88
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:189
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::size_type size_type
Definition: fwd.hh:173
Definition: bi-rrt-planner.hh:35