hpp-core  4.11.0
Implement basic classes for canonical path planning for kinematic chains.
polynomial.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-core.
5 // hpp-core is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-core is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-core. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
18 # define HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
19 
20 # include <hpp/constraints/differentiable-function.hh>
21 
22 # include <hpp/core/fwd.hh>
23 # include <hpp/core/config.hh>
25 # include <hpp/core/path/math.hh>
26 
27 namespace hpp {
28  namespace core {
29  namespace timeParameterization {
31  {
32  public:
33  Polynomial (const vector_t& param) : a (param)
34  {
35  for (size_type i=0; i<a.size(); ++i){
36  assert(a[i] < std::numeric_limits<value_type>::infinity());
37  assert(a[i] > -std::numeric_limits<value_type>::infinity());
38  }
39  }
40 
41  const vector_t& parameters () const
42  {
43  return a;
44  }
45 
47  {
48  return TimeParameterizationPtr_t (new Polynomial (*this));
49  }
50 
52  value_type value (const value_type& t) const
53  {
54  return val (t);
55  }
56 
58  value_type derivative (const value_type& t, const size_type& order) const
59  {
60  return Jac(t, order);
61  }
62 
74  value_type derivativeBound (const value_type& low, const value_type& up) const
75  {
76  using std::max;
77  using std::fabs;
78  switch (a.size()) {
79  case 2:
80  return fabs(a[1]);
81  break;
82  case 3:
83  return max (fabs(Jac(low)), fabs(Jac(up)));
84  break;
85  case 4:
86  {
87  const value_type x_m = - a[2] / (3 * a[3]);
88  const value_type M = max(fabs(Jac(low)), fabs(Jac(up)));
89  if (low < x_m && x_m < up)
90  return max (M, fabs(a[1] - a[2] / 3*a[3]));
91  else
92  return M;
93  }
94  break;
95  default:
96  throw std::logic_error("not implemented");
97  }
98  }
99 
100  private:
101  value_type val (const value_type& t) const
102  {
103  value_type tn = 1;
104  value_type res = a[0];
105  for (size_type i = 1; i < a.size(); ++i)
106  {
107  tn *= t;
108  res += a[i] * tn;
109  }
110  assert (res == res);
111  return res;
112  }
113 
114  value_type Jac (const value_type& t) const
115  {
116  return Jac(t,1);
117  }
118 
119  value_type Jac (const value_type& t, const size_type& order) const
120  {
121  if (order >= a.size()) return 0;
122  const size_type MaxOrder = 10;
123  if (a.size() > MaxOrder)
124  throw std::invalid_argument ("Cannot compute the derivative of order greater than 10.");
125  typedef path::binomials<MaxOrder> Binomials_t;
126  const Binomials_t::Factorials_t& factors = Binomials_t::factorials();
127 
128  value_type res = 0;
129  value_type tn = 1;
130  for (size_type i = order; i < a.size(); ++i)
131  {
132  res += value_type(factors[i]/factors[i-order]) * a[i] * tn;
133  tn *= t;
134  }
135  return res;
136  }
137 
138  vector_t a;
139  }; // class Polynomial
140  } // namespace timeParameterization
141  } // namespace core
142 } // namespace hpp
143 #endif // HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:172
Definition: bi-rrt-planner.hh:24
TimeParameterizationPtr_t copy() const
Definition: polynomial.hh:46
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition: polynomial.hh:58
pinocchio::size_type size_type
Definition: fwd.hh:156
Definition: time-parameterization.hh:25
pinocchio::vector_t vector_t
Definition: fwd.hh:202
pinocchio::value_type value_type
Definition: fwd.hh:157
value_type derivativeBound(const value_type &low, const value_type &up) const
Definition: polynomial.hh:74
Polynomial(const vector_t &param)
Definition: polynomial.hh:33
const vector_t & parameters() const
Definition: polynomial.hh:41
#define HPP_CORE_DLLAPI
Definition: config.hh:64
value_type value(const value_type &t) const
Computes .
Definition: polynomial.hh:52
Definition: math.hh:23