18 #ifndef HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH 19 # define HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH 21 #include <hpp/constraints/differentiable-function.hh> 30 namespace timeParameterization {
41 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor>
ParameterMatrix_t;
42 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1>
Vector_t;
51 parameters_ (parameters),
52 breakpoints_(breakpoints)
54 assert(breakpoints_.size() == parameters_.cols()+1);
55 assert(parameters_.rows() == NbCoeffs);
57 for (
size_type j=0; j<parameters_.cols(); ++j){
59 assert(breakpoints_[j] > breakpoints_[j-1]);
61 for (
size_type i=0; i<parameters_.rows(); ++i){
62 assert(parameters_(i,j) < std::numeric_limits<value_type>::infinity());
63 assert(parameters_(i,j) > -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)
101 res += poly_coeffs[i] * tn;
114 if (order >= parameters_.rows())
return 0;
116 if (parameters_.rows() > MaxOrder)
117 throw std::invalid_argument (
"Cannot compute the derivative of order greater than 10.");
118 typedef path::binomials<MaxOrder> Binomials_t;
119 const Binomials_t::Factorials_t& factors = Binomials_t::factorials();
121 const size_t seg_index = findPolynomialIndex(t);
122 const auto& poly_coeffs = parameters_.col(seg_index);
126 for (
size_type i = order; i < poly_coeffs.size(); ++i)
128 res +=
value_type(factors[i]/factors[i-order]) * poly_coeffs[i] * tn;
134 size_t findPolynomialIndex(
const value_type& t)
const {
135 size_t seg_index = std::numeric_limits<size_t>::max();
136 for (
int i = 0; i < parameters_.size(); ++i) {
137 if (breakpoints_[i] <= t && t <= breakpoints_[i + 1]) {
142 if (seg_index == std::numeric_limits<size_t>::max()) {
143 std::ostringstream oss;
144 oss <<
"Position " << t <<
" is outside of range [ " << breakpoints_[0]
145 <<
", " << breakpoints_[breakpoints_.size()-1] <<
']';
146 throw std::runtime_error(oss.str());
154 ParameterMatrix_t parameters_;
155 Vector_t breakpoints_;
160 #endif // HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:172
const ParameterMatrix_t & parameters() const
Definition: piecewise-polynomial.hh:69
Definition: bi-rrt-planner.hh:24
Definition: piecewise-polynomial.hh:33
pinocchio::size_type size_type
Definition: fwd.hh:156
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor > ParameterMatrix_t
Definition: piecewise-polynomial.hh:41
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > Vector_t
Definition: piecewise-polynomial.hh:42
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition: piecewise-polynomial.hh:86
Definition: time-parameterization.hh:25
pinocchio::value_type value_type
Definition: fwd.hh:157
PiecewisePolynomial(const ParameterMatrix_t ¶meters, const Vector_t &breakpoints)
Definition: piecewise-polynomial.hh:49
TimeParameterizationPtr_t copy() const
Definition: piecewise-polynomial.hh:74
#define HPP_CORE_DLLAPI
Definition: config.hh:64
value_type value(const value_type &t) const
Computes .
Definition: piecewise-polynomial.hh:80