hpp-core: include/hpp/core/time-parameterization/polynomial.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
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
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
30#define HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
31
32#include <hpp/constraints/differentiable-function.hh>
33#include <hpp/core/config.hh>
34#include <hpp/core/fwd.hh>
35#include <hpp/core/path/math.hh>
36#include <hpp/core/time-parameterization.hh>
37
38namespace hpp {
39namespace core {
40namespace timeParameterization {
41 class HPP_CORE_DLLAPI Polynomial : public TimeParameterization {
42 public:
43 Polynomial(const vector_t& param) : a(param) {
44 for (size_type i = 0; i < a.size(); ++i) {
45 assert(a[i] < std::numeric_limits<value_type>::infinity());
46 assert(a[i] > -std::numeric_limits<value_type>::infinity());
47 }
48 }
49
50 const vector_t& parameters() const { return a; }
51
52 TimeParameterizationPtr_t copy() const {
53 return TimeParameterizationPtr_t(new Polynomial(*this));
54 }
55
57 value_type value(const value_type& t) const { return val(t); }
58
60 value_type derivative(const value_type& t, const size_type& order) const {
61 return Jac(t, order);
62 }
63
75 value_type derivativeBound(const value_type& low,
76 const value_type& up) const {
77 using std::fabs;
78 using std::max;
79 switch (a.size()) {
80 case 2:
81 return fabs(a[1]);
82 break;
83 case 3:
84 return max(fabs(Jac(low)), fabs(Jac(up)));
85 break;
86 case 4: {
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 } break;
94 default:
95 throw std::logic_error("not implemented");
96 }
97 }
98
99 private:
100 value_type val(const value_type& t) const {
101 value_type tn = 1;
102 value_type res = a[0];
103 for (size_type i = 1; i < a.size(); ++i) {
104 tn *= t;
105 res += a[i] * tn;
106 }
107 assert(res == res);
108 return res;
109 }
110
111 value_type Jac(const value_type& t) const { return Jac(t, 1); }
112
113 value_type Jac(const value_type& t, const size_type& order) const {
114 if (order >= a.size()) return 0;
115 const size_type MaxOrder = 10;
116 if (a.size() > MaxOrder)
117 throw std::invalid_argument(
118 "Cannot compute the derivative of order greater than 10.");
119 typedef path::binomials<MaxOrder> Binomials_t;
120 const Binomials_t::Factorials_t& factors = Binomials_t::factorials();
121
122 value_type res = 0;
123 value_type tn = 1;
124 for (size_type i = order; i < a.size(); ++i) {
125 res += value_type(factors[i] / factors[i - order]) * a[i] * tn;
126 tn *= t;
127 }
128 return res;
129 }
130
131 vector_t a;
132}; // class Polynomial
133} // namespace timeParameterization
134} // namespace core
135} // namespace hpp
136#endif // HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH
hpp::core::TimeParameterization
Definition time-parameterization.hh:37
hpp::core::timeParameterization::Polynomial
Definition polynomial.hh:41
hpp::core::timeParameterization::Polynomial::copy
TimeParameterizationPtr_t copy() const
Definition polynomial.hh:52
hpp::core::timeParameterization::Polynomial::value
value_type value(const value_type &t) const
Computes .
Definition polynomial.hh:57
hpp::core::timeParameterization::Polynomial::derivative
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition polynomial.hh:60
hpp::core::timeParameterization::Polynomial::derivativeBound
value_type derivativeBound(const value_type &low, const value_type &up) const
Definition polynomial.hh:75
hpp::core::timeParameterization::Polynomial::Polynomial
Polynomial(const vector_t &param)
Definition polynomial.hh:43
hpp::core::timeParameterization::Polynomial::parameters
const vector_t & parameters() const
Definition polynomial.hh:50
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
differentiable-function.hh
fwd.hh
math.hh
hpp
hpp::constraints::vector_t
pinocchio::vector_t vector_t
hpp::core::TimeParameterizationPtr_t
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition fwd.hh:189
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
time-parameterization.hh