hpp-core 4.15.1
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>
38
39namespace hpp {
40namespace core {
41namespace timeParameterization {
42
43template <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
Definition: time-parameterization.hh:37
Definition: piecewise-polynomial.hh:44
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > Vector_t
Definition: piecewise-polynomial.hh:53
value_type value(const value_type &t) const
Computes .
Definition: piecewise-polynomial.hh:85
const ParameterMatrix_t & parameters() const
Definition: piecewise-polynomial.hh:78
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::ColMajor > ParameterMatrix_t
Definition: piecewise-polynomial.hh:52
PiecewisePolynomial(const ParameterMatrix_t &parameters, const Vector_t &breakpoints)
Definition: piecewise-polynomial.hh:60
TimeParameterizationPtr_t copy() const
Definition: piecewise-polynomial.hh:80
value_type derivative(const value_type &t, const size_type &order) const
Computes .
Definition: piecewise-polynomial.hh:88
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:189
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::size_type size_type
Definition: fwd.hh:173
Definition: bi-rrt-planner.hh:35