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

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

Loading...
Searching...
No Matches
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>
37#include <hpp/core/time-parameterization.hh>
38
39namespace hpp {
40namespace core {
41 namespace timeParameterization {
42
43template <int _Order>
44 class HPP_CORE_DLLAPI PiecewisePolynomial : public TimeParameterization {
45 public:
46 enum {
47 Order = _Order,
48 NbCoeffs = Order + 1,
49 };
50
51 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor>
52 ParameterMatrix_t;
53 typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> Vector_t;
54
60 PiecewisePolynomial(const ParameterMatrix_t& parameters,
61 const Vector_t& breakpoints)
62 : parameters_(parameters),
63 breakpoints_(breakpoints.data(),
64 breakpoints.data() + breakpoints.size()) {
65 assert(size_type(breakpoints_.size()) == parameters_.cols() + 1);
66 assert(parameters_.rows() == NbCoeffs);
67
68 for (size_type j = 0; j < parameters_.cols(); ++j) {
69 if (j > 0) {
70 assert(breakpoints_[j] > breakpoints_[j - 1]);
71 }
72 for (size_type i = 0; i < parameters_.rows(); ++i) {
73 assert(parameters_(i, j) < std::numeric_limits<value_type>::infinity());
74 assert(parameters_(i, j) >
75 -std::numeric_limits<value_type>::infinity());
76 }
77 }
78 }
79
80 const ParameterMatrix_t& parameters() const { return parameters_; }
81
82 const std::vector<value_type>& breakpoints() const { return breakpoints_; }
83
84 TimeParameterizationPtr_t copy() const {
85 return TimeParameterizationPtr_t(new PiecewisePolynomial(*this));
86 }
87
89 value_type value(const value_type& t) const { return val(t); }
90
92 value_type derivative(const value_type& t, const size_type& order) const {
93 return Jac(t, order);
94 }
95
104 bool polynomialsStartAtZero() const { return startAtZero_; }
105
107 void polynomialsStartAtZero(bool startAtZero) { startAtZero_ = startAtZero; }
108
109 private:
110 value_type val(value_type t) const {
111 const size_t seg_index = findPolynomialIndex(t);
112 const auto& poly_coeffs = parameters_.col(seg_index);
113 value_type tn = 1;
114 value_type res = poly_coeffs[0];
115 for (size_type i = 1; i < poly_coeffs.size(); ++i) {
116 tn *= t;
117 res += poly_coeffs[i] * tn;
118 }
119 assert(res == res);
120 return res;
121 }
122
123 value_type Jac(const value_type& t) const { return Jac(t, 1); }
124
125 value_type Jac(value_type t, const size_type& order) const {
126 if (order >= parameters_.rows()) return 0;
127 const size_type MaxOrder = 10;
128 if (parameters_.rows() > MaxOrder)
129 throw std::invalid_argument(
130 "Cannot compute the derivative of order greater than 10.");
131 typedef path::binomials<MaxOrder> Binomials_t;
132 const Binomials_t::Factorials_t& factors = Binomials_t::factorials();
133
134 const size_t seg_index = findPolynomialIndex(t);
135 const auto& poly_coeffs = parameters_.col(seg_index);
136
137 value_type res = 0;
138 value_type tn = 1;
139 for (size_type i = order; i < poly_coeffs.size(); ++i) {
140 res += value_type(factors[i] / factors[i - order]) * poly_coeffs[i] * tn;
141 tn *= t;
142 }
143 return res;
144 }
145
146 size_t findPolynomialIndex(value_type& t) const {
147 size_t index;
148
149 // Points to the smallest element of breakpoints_ that is strictly greater
150 // than t
151 auto breakpointIter = std::lower_bound(
152 breakpoints_.begin(), breakpoints_.end(), t, std::less_equal<double>());
153 if (breakpointIter == breakpoints_.begin()) {
154 // t is smaller than breakpoints_[0]
155 assert(t < breakpoints_[0]);
156 // Should we handle numerical issues, i.e. t is very close to
157 // breakpoints_[0] ?
158 index = breakpoints_.size();
159 } else if (breakpointIter == breakpoints_.end()) {
160 if (t > breakpoints_.back())
161 index = breakpoints_.size();
162 else
163 index = breakpoints_.size() - 2;
164 } else {
165 index = std::distance(breakpoints_.begin(), breakpointIter) - 1;
166 }
167 if (index == breakpoints_.size()) {
168 std::ostringstream oss;
169 oss << "Position " << t << " is outside of range [ " << breakpoints_[0]
170 << ", " << breakpoints_[breakpoints_.size() - 1] << ']';
171 throw std::invalid_argument(oss.str());
172 }
173 if (startAtZero_) {
174 t -= breakpoints_[index];
175 }
176 return index;
177 }
178
182 ParameterMatrix_t parameters_;
183 std::vector<value_type> breakpoints_; // size N + 1
185 bool startAtZero_ = false;
186}; // class PiecewisePolynomial
187} // namespace timeParameterization
188} // namespace core
189} // namespace hpp
190#endif // HPP_CORE_TIME_PARAMETERIZATION_PIECEWISE_POLYNOMIAL_HH
hpp::core::TimeParameterization
Definition time-parameterization.hh:37
hpp::core::timeParameterization::PiecewisePolynomial
Definition piecewise-polynomial.hh:44
hpp::core::timeParameterization::PiecewisePolynomial::Vector_t
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > Vector_t
Definition piecewise-polynomial.hh:53
hpp::core::timeParameterization::PiecewisePolynomial::polynomialsStartAtZero
void polynomialsStartAtZero(bool startAtZero)
See the corresponding getter.
Definition piecewise-polynomial.hh:107
hpp::core::timeParameterization::PiecewisePolynomial::value
value_type value(const value_type &t) const
Computes .
Definition piecewise-polynomial.hh:89
hpp::core::timeParameterization::PiecewisePolynomial::parameters
const ParameterMatrix_t & parameters() const
Definition piecewise-polynomial.hh:80
hpp::core::timeParameterization::PiecewisePolynomial::ParameterMatrix_t
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor > ParameterMatrix_t
Definition piecewise-polynomial.hh:52
hpp::core::timeParameterization::PiecewisePolynomial::PiecewisePolynomial
PiecewisePolynomial(const ParameterMatrix_t &parameters, const Vector_t &breakpoints)
Definition piecewise-polynomial.hh:60
hpp::core::timeParameterization::PiecewisePolynomial::breakpoints
const std::vector< value_type > & breakpoints() const
Definition piecewise-polynomial.hh:82
hpp::core::timeParameterization::PiecewisePolynomial::copy
TimeParameterizationPtr_t copy() const
Definition piecewise-polynomial.hh:84
hpp::core::timeParameterization::PiecewisePolynomial::polynomialsStartAtZero
bool polynomialsStartAtZero() const
Definition piecewise-polynomial.hh:104
hpp::core::timeParameterization::PiecewisePolynomial::derivative
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition piecewise-polynomial.hh:92
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
differentiable-function.hh
fwd.hh
math.hh
Eigen::assert
assert(d.lhs()._blocks()==d.rhs()._blocks())
hpp
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::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
time-parameterization.hh