hpp-core  4.15.1
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 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29 
30 #ifndef HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
31 #define HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
32 
33 #include <hpp/constraints/differentiable-function.hh>
34 #include <hpp/core/config.hh>
35 #include <hpp/core/fwd.hh>
36 #include <hpp/core/path/math.hh>
38 
39 namespace hpp {
40 namespace core {
41 namespace timeParameterization {
42 
43 template <int _Order>
45  public:
46  enum {
47  Order = _Order,
48  NbCoeffs = Order + 1,
49  };
50 
51  typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor>
53  typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> Vector_t;
54 
61  const Vector_t& breakpoints)
62  : parameters_(parameters), breakpoints_(breakpoints) {
63  assert(breakpoints_.size() == parameters_.cols() + 1);
64  assert(parameters_.rows() == NbCoeffs);
65 
66  for (size_type j = 0; j < parameters_.cols(); ++j) {
67  if (j > 0) {
68  assert(breakpoints_[j] > breakpoints_[j - 1]);
69  }
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());
74  }
75  }
76  }
77 
78  const ParameterMatrix_t& parameters() const { return parameters_; }
79 
82  }
83 
85  value_type value(const value_type& t) const { return val(t); }
86 
88  value_type derivative(const value_type& t, const size_type& order) const {
89  return Jac(t, order);
90  }
91 
92  private:
93  value_type val(const value_type& t) const {
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  tn *= t;
100  res += poly_coeffs[i] * tn;
101  }
102  assert(res == res);
103  return res;
104  }
105 
106  value_type Jac(const value_type& t) const { return Jac(t, 1); }
107 
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();
116 
117  const size_t seg_index = findPolynomialIndex(t);
118  const auto& poly_coeffs = parameters_.col(seg_index);
119 
120  value_type res = 0;
121  value_type tn = 1;
122  for (size_type i = order; i < poly_coeffs.size(); ++i) {
123  res += value_type(factors[i] / factors[i - order]) * poly_coeffs[i] * tn;
124  tn *= t;
125  }
126  return res;
127  }
128 
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]) {
133  seg_index = i;
134  break;
135  }
136  }
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());
142  }
143  return seg_index;
144  }
145 
149  ParameterMatrix_t parameters_;
150  Vector_t breakpoints_; // size N + 1
151 }; // class PiecewisePolynomial
152 } // namespace timeParameterization
153 } // namespace core
154 } // namespace hpp
155 #endif // HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:189
const ParameterMatrix_t & parameters() const
Definition: piecewise-polynomial.hh:78
Definition: bi-rrt-planner.hh:35
Definition: piecewise-polynomial.hh:44
pinocchio::size_type size_type
Definition: fwd.hh:173
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor > ParameterMatrix_t
Definition: piecewise-polynomial.hh:52
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > Vector_t
Definition: piecewise-polynomial.hh:53
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition: piecewise-polynomial.hh:88
Definition: time-parameterization.hh:37
pinocchio::value_type value_type
Definition: fwd.hh:174
PiecewisePolynomial(const ParameterMatrix_t &parameters, const Vector_t &breakpoints)
Definition: piecewise-polynomial.hh:60
TimeParameterizationPtr_t copy() const
Definition: piecewise-polynomial.hh:80
#define HPP_CORE_DLLAPI
Definition: config.hh:64
Definition: math.hh:39
value_type value(const value_type &t) const
Computes .
Definition: piecewise-polynomial.hh:85