hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
piecewise-polynomial.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 // Olivier Roussel (olivier.roussel@laas.fr)
4 //
5 // This file is part of hpp-core.
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core. If not, see <http://www.gnu.org/licenses/>.
17 
18 #ifndef HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
19 # define HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
20 
21 #include <hpp/constraints/differentiable-function.hh>
22 
23 #include <hpp/core/fwd.hh>
24 #include <hpp/core/config.hh>
26 #include <hpp/core/path/math.hh>
27 
28 namespace hpp {
29  namespace core {
30  namespace timeParameterization {
31 
32  template <int _Order>
34  {
35  public:
36  enum {
37  Order = _Order,
38  NbCoeffs = Order + 1,
39  };
40 
41  typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor> ParameterMatrix_t;
42  typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> Vector_t;
43 
49  PiecewisePolynomial (const ParameterMatrix_t& parameters,
50  const Vector_t& breakpoints) :
51  parameters_ (parameters),
52  breakpoints_(breakpoints)
53  {
54  assert(breakpoints_.size() == parameters_.cols()+1);
55  assert(parameters_.rows() == NbCoeffs);
56 
57  for (size_type j=0; j<parameters_.cols(); ++j){
58  if (j > 0){
59  assert(breakpoints_[j] > breakpoints_[j-1]);
60  }
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());
64  }
65  }
66 
67  }
68 
69  const ParameterMatrix_t& parameters () const
70  {
71  return parameters_;
72  }
73 
75  {
77  }
78 
80  value_type value (const value_type& t) const
81  {
82  return val (t);
83  }
84 
86  value_type derivative (const value_type& t, const size_type& order) const
87  {
88  return Jac(t, order);
89  }
90 
91  private:
92  value_type val (const value_type& t) const
93  {
94  const size_t seg_index = findPolynomialIndex(t);
95  const auto& poly_coeffs = parameters_.col(seg_index);
96  value_type tn = 1;
97  value_type res = poly_coeffs[0];
98  for (size_type i = 1; i < poly_coeffs.size(); ++i)
99  {
100  tn *= t;
101  res += poly_coeffs[i] * tn;
102  }
103  assert (res == res);
104  return res;
105  }
106 
107  value_type Jac (const value_type& t) const
108  {
109  return Jac(t,1);
110  }
111 
112  value_type Jac (const value_type& t, const size_type& order) const
113  {
114  if (order >= parameters_.rows()) return 0;
115  const size_type MaxOrder = 10;
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();
120 
121  const size_t seg_index = findPolynomialIndex(t);
122  const auto& poly_coeffs = parameters_.col(seg_index);
123 
124  value_type res = 0;
125  value_type tn = 1;
126  for (size_type i = order; i < poly_coeffs.size(); ++i)
127  {
128  res += value_type(factors[i]/factors[i-order]) * poly_coeffs[i] * tn;
129  tn *= t;
130  }
131  return res;
132  }
133 
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]) {
138  seg_index = i;
139  break;
140  }
141  }
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());
147  }
148  return seg_index;
149  }
150 
154  ParameterMatrix_t parameters_;
155  Vector_t breakpoints_; // size N + 1
156  }; // class PiecewisePolynomial
157  } // namespace timeParameterization
158  } // namespace core
159 } // namespace hpp
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 &parameters, 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
Definition: math.hh:23
value_type value(const value_type &t) const
Computes .
Definition: piecewise-polynomial.hh:80