Directory: | ./ |
---|---|
File: | include/hpp/core/time-parameterization/piecewise-polynomial.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 60 | 63 | 95.2% |
Branches: | 34 | 60 | 56.7% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | |||
39 | namespace hpp { | ||
40 | namespace core { | ||
41 | namespace timeParameterization { | ||
42 | |||
43 | template <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 | |||
55 | /** Construct piecewise polynomials of order k and defined by N polynomial. | ||
56 | * \param parameters Matrix of polynomials coefficients (size k x N). | ||
57 | * \param breakpoints Domain of the piecewise polynomial, defined by N+1 | ||
58 | * breakpoints defining the half-open intervals of each polynomial. | ||
59 | */ | ||
60 | 4 | PiecewisePolynomial(const ParameterMatrix_t& parameters, | |
61 | const Vector_t& breakpoints) | ||
62 | 4 | : parameters_(parameters), | |
63 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
8 | breakpoints_(breakpoints.data(), |
64 |
2/4✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
8 | breakpoints.data() + breakpoints.size()) { |
65 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
|
4 | assert(size_type(breakpoints_.size()) == parameters_.cols() + 1); |
66 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
4 | assert(parameters_.rows() == NbCoeffs); |
67 | |||
68 |
2/2✓ Branch 1 taken 5 times.
✓ Branch 2 taken 2 times.
|
14 | for (size_type j = 0; j < parameters_.cols(); ++j) { |
69 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
|
10 | if (j > 0) { |
70 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
|
6 | assert(breakpoints_[j] > breakpoints_[j - 1]); |
71 | } | ||
72 |
2/2✓ Branch 1 taken 13 times.
✓ Branch 2 taken 5 times.
|
36 | for (size_type i = 0; i < parameters_.rows(); ++i) { |
73 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 13 times.
|
26 | assert(parameters_(i, j) < std::numeric_limits<value_type>::infinity()); |
74 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 13 times.
|
26 | assert(parameters_(i, j) > |
75 | -std::numeric_limits<value_type>::infinity()); | ||
76 | } | ||
77 | } | ||
78 | 4 | } | |
79 | |||
80 | const ParameterMatrix_t& parameters() const { return parameters_; } | ||
81 | |||
82 | 1 | const std::vector<value_type>& breakpoints() const { return breakpoints_; } | |
83 | |||
84 | ✗ | TimeParameterizationPtr_t copy() const { | |
85 | ✗ | return TimeParameterizationPtr_t(new PiecewisePolynomial(*this)); | |
86 | } | ||
87 | |||
88 | /// Computes \f$ \sum_{i=0}^n a_i t^i \f$ | ||
89 | 30 | value_type value(const value_type& t) const { return val(t); } | |
90 | |||
91 | /// Computes \f$ \sum_{i=1}^n i a_i t^{i-1} \f$ | ||
92 | 20004 | value_type derivative(const value_type& t, const size_type& order) const { | |
93 | 20004 | return Jac(t, order); | |
94 | } | ||
95 | |||
96 | /// Whether the polynomial should be shifted. | ||
97 | /// | ||
98 | /// If \c true, when evaluating the polynomials, the initial breakpoint time | ||
99 | /// of a polynomial is substracted. A polynomial defined between | ||
100 | /// \f$ [ t_k, t_{k+1} [ \f$ evaluates to | ||
101 | /// \f$ P(t) = \sum_{i=0}^n a_i (t - t_k)^i \f$. | ||
102 | /// | ||
103 | /// This defaults to \c false. | ||
104 | bool polynomialsStartAtZero() const { return startAtZero_; } | ||
105 | |||
106 | /// See the corresponding getter. | ||
107 | 1 | void polynomialsStartAtZero(bool startAtZero) { startAtZero_ = startAtZero; } | |
108 | |||
109 | private: | ||
110 | 30 | value_type val(value_type t) const { | |
111 |
2/2✓ Branch 1 taken 13 times.
✓ Branch 2 taken 2 times.
|
30 | const size_t seg_index = findPolynomialIndex(t); |
112 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | const auto& poly_coeffs = parameters_.col(seg_index); |
113 | 26 | value_type tn = 1; | |
114 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | value_type res = poly_coeffs[0]; |
115 |
2/2✓ Branch 1 taken 23 times.
✓ Branch 2 taken 13 times.
|
72 | for (size_type i = 1; i < poly_coeffs.size(); ++i) { |
116 | 46 | tn *= t; | |
117 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
46 | res += poly_coeffs[i] * tn; |
118 | } | ||
119 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
|
26 | assert(res == res); |
120 | 26 | return res; | |
121 | } | ||
122 | |||
123 | value_type Jac(const value_type& t) const { return Jac(t, 1); } | ||
124 | |||
125 | 20004 | value_type Jac(value_type t, const size_type& order) const { | |
126 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 10002 times.
|
20004 | if (order >= parameters_.rows()) return 0; |
127 | 20004 | const size_type MaxOrder = 10; | |
128 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 10002 times.
|
20004 | 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 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
20004 | const Binomials_t::Factorials_t& factors = Binomials_t::factorials(); |
133 | |||
134 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
20004 | const size_t seg_index = findPolynomialIndex(t); |
135 |
1/2✓ Branch 1 taken 10002 times.
✗ Branch 2 not taken.
|
20004 | const auto& poly_coeffs = parameters_.col(seg_index); |
136 | |||
137 | 20004 | value_type res = 0; | |
138 | 20004 | value_type tn = 1; | |
139 |
2/2✓ Branch 1 taken 20004 times.
✓ Branch 2 taken 10002 times.
|
60012 | for (size_type i = order; i < poly_coeffs.size(); ++i) { |
140 |
3/6✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20004 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20004 times.
✗ Branch 8 not taken.
|
40008 | res += value_type(factors[i] / factors[i - order]) * poly_coeffs[i] * tn; |
141 | 40008 | tn *= t; | |
142 | } | ||
143 | 20004 | return res; | |
144 | } | ||
145 | |||
146 | 10017 | 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 | 10017 | auto breakpointIter = std::lower_bound( | |
152 | breakpoints_.begin(), breakpoints_.end(), t, std::less_equal<double>()); | ||
153 | 10017 | if (breakpointIter == breakpoints_.begin()) { | |
154 | // t is smaller than breakpoints_[0] | ||
155 | 1 | assert(t < breakpoints_[0]); | |
156 | // Should we handle numerical issues, i.e. t is very close to | ||
157 | // breakpoints_[0] ? | ||
158 | 1 | index = breakpoints_.size(); | |
159 | 10016 | } else if (breakpointIter == breakpoints_.end()) { | |
160 | 4 | if (t > breakpoints_.back()) | |
161 | 1 | index = breakpoints_.size(); | |
162 | else | ||
163 | 3 | index = breakpoints_.size() - 2; | |
164 | } else { | ||
165 | 10012 | index = std::distance(breakpoints_.begin(), breakpointIter) - 1; | |
166 | } | ||
167 | 10017 | if (index == breakpoints_.size()) { | |
168 | 2 | std::ostringstream oss; | |
169 | 2 | oss << "Position " << t << " is outside of range [ " << breakpoints_[0] | |
170 | 2 | << ", " << breakpoints_[breakpoints_.size() - 1] << ']'; | |
171 | 2 | throw std::invalid_argument(oss.str()); | |
172 | 2 | } | |
173 | 10015 | if (startAtZero_) { | |
174 | 10010 | t -= breakpoints_[index]; | |
175 | } | ||
176 | 10015 | return index; | |
177 | } | ||
178 | |||
179 | /// Parameters of the polynomials are stored in a matrix | ||
180 | /// number of rows = degree of polynomial + 1 | ||
181 | /// number of columns = number of polynomials N | ||
182 | ParameterMatrix_t parameters_; | ||
183 | std::vector<value_type> breakpoints_; // size N + 1 | ||
184 | /// See the corresponding getter. | ||
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 | ||
191 |