GCC Code Coverage Report

Directory: ./
File: include/hpp/core/time-parameterization/piecewise-polynomial.hh
Date: 2024-12-13 16:14:03
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