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