Directory: | ./ |
---|---|
File: | src/path/spline.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 172 | 211 | 81.5% |
Branches: | 231 | 510 | 45.3% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | #include <hpp/core/path/math.hh> | ||
30 | #include <hpp/core/path/spline.hh> | ||
31 | #include <hpp/pinocchio/configuration.hh> | ||
32 | #include <hpp/pinocchio/liegroup-space.hh> | ||
33 | #include <hpp/pinocchio/liegroup.hh> | ||
34 | #include <hpp/pinocchio/serialization.hh> | ||
35 | #include <hpp/util/serialization.hh> | ||
36 | #include <pinocchio/serialization/eigen.hpp> | ||
37 | |||
38 | namespace hpp { | ||
39 | namespace core { | ||
40 | namespace path { | ||
41 | namespace internal { | ||
42 | /// Spline basis functions input set is [0, 1] | ||
43 | template <int Degree> | ||
44 | struct spline_basis_function<CanonicalPolynomeBasis, Degree> { | ||
45 | typedef sbf_traits<CanonicalPolynomeBasis, Degree> traits; | ||
46 | enum { NbCoeffs = traits::NbCoeffs }; | ||
47 | typedef Eigen::Matrix<size_type, NbCoeffs, 1> Factorials_t; | ||
48 | typedef typename traits::Coeffs_t Coeffs_t; | ||
49 | typedef typename traits::IntegralCoeffs_t IntegralCoeffs_t; | ||
50 | |||
51 | static void eval(const value_type t, Coeffs_t& res); | ||
52 | static void derivative(const size_type order, const value_type& t, | ||
53 | Coeffs_t& res); | ||
54 | /// Integrate between 0 and 1 | ||
55 | static void integral(const size_type order, IntegralCoeffs_t& res); | ||
56 | static void absDerBounds(Coeffs_t& res); | ||
57 | static void bound(const size_type& order, const value_type& /* t0 */, | ||
58 | const value_type& t1, Coeffs_t& res) { | ||
59 | derivative(order, t1, res); | ||
60 | } | ||
61 | }; | ||
62 | template <int Degree> | ||
63 | void spline_basis_function<CanonicalPolynomeBasis, Degree>::eval( | ||
64 | const value_type t, Coeffs_t& res) { | ||
65 | res(0) = 1; | ||
66 | for (size_type i = 1; i < NbCoeffs; ++i) res(i) = res(i - 1) * t; | ||
67 | } | ||
68 | template <int Degree> | ||
69 | void spline_basis_function<CanonicalPolynomeBasis, Degree>::derivative( | ||
70 | const size_type order, const value_type& t, Coeffs_t& res) { | ||
71 | static Factorials_t factors = binomials<NbCoeffs>::factorials(); | ||
72 | value_type powerOfT = 1; | ||
73 | res.head(order).setZero(); | ||
74 | for (size_type i = order; i < NbCoeffs; ++i) { | ||
75 | res(i) = value_type(factors(i) / factors(i - order)) * powerOfT; | ||
76 | powerOfT *= t; | ||
77 | } | ||
78 | } | ||
79 | template <int Degree> | ||
80 | void spline_basis_function<CanonicalPolynomeBasis, Degree>::integral( | ||
81 | const size_type order, IntegralCoeffs_t& res) { | ||
82 | static Factorials_t factors = binomials<NbCoeffs>::factorials(); | ||
83 | |||
84 | // TODO the output matrix is symmetric | ||
85 | if (order > 0) { | ||
86 | res.topRows(order).setZero(); | ||
87 | res.leftCols(order).setZero(); | ||
88 | } | ||
89 | |||
90 | for (size_type i = order; i < NbCoeffs; ++i) { | ||
91 | // TODO size_type + cache this values. | ||
92 | const size_type factor_i(factors(i) / factors(i - order)); | ||
93 | for (size_type j = order; j < NbCoeffs; ++j) { | ||
94 | // TODO size_type + cache this values. | ||
95 | const size_type factor_j(factors(j) / factors(j - order)); | ||
96 | const size_type power = i + j - 2 * order + 1; | ||
97 | res(i, j) = value_type(factor_i * factor_j) / value_type(power); | ||
98 | } | ||
99 | } | ||
100 | } | ||
101 | template <int Degree> | ||
102 | void spline_basis_function<CanonicalPolynomeBasis, Degree>::absDerBounds( | ||
103 | Coeffs_t& res) { | ||
104 | res(0) = 0; | ||
105 | for (size_type i = 1; i < NbCoeffs; ++i) res(i) = value_type(i); | ||
106 | } | ||
107 | |||
108 | template <int Degree> | ||
109 | struct spline_basis_function<BernsteinBasis, Degree> { | ||
110 | enum { NbCoeffs = Degree + 1 }; | ||
111 | typedef Eigen::Matrix<size_type, NbCoeffs, 1> Factorials_t; | ||
112 | typedef Eigen::Matrix<value_type, NbCoeffs, 1> Coeffs_t; | ||
113 | typedef Eigen::Matrix<value_type, NbCoeffs, NbCoeffs> IntegralCoeffs_t; | ||
114 | |||
115 | static void eval(const value_type t, Coeffs_t& res); | ||
116 | static void derivative(const size_type order, const value_type& t, | ||
117 | Coeffs_t& res); | ||
118 | /// Integrate between 0 and 1 | ||
119 | static void integral(const size_type order, IntegralCoeffs_t& res); | ||
120 | static void absDerBounds(Coeffs_t& res); | ||
121 | static void bound(const size_type& order, const value_type& t0, | ||
122 | const value_type& t1, Coeffs_t& res); | ||
123 | |||
124 | private: | ||
125 | static Coeffs_t absBound(bool up); | ||
126 | }; | ||
127 | template <int Degree> | ||
128 | void spline_basis_function<BernsteinBasis, Degree>::eval(const value_type t, | ||
129 | Coeffs_t& res) { | ||
130 | derivative(0, t, res); | ||
131 | } | ||
132 | template <int Degree> | ||
133 | 5661070 | void spline_basis_function<BernsteinBasis, Degree>::derivative( | |
134 | const size_type k, const value_type& t, Coeffs_t& res) { | ||
135 |
2/4✓ Branch 0 taken 2830544 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 2830546 times.
✗ Branch 3 not taken.
|
5661070 | assert(0 <= t && t <= 1); |
136 |
1/2✓ Branch 1 taken 2830690 times.
✗ Branch 2 not taken.
|
5661092 | res.setZero(); |
137 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2830690 times.
|
5661380 | if (k > Degree) return; |
138 |
6/10✓ Branch 0 taken 18 times.
✓ Branch 1 taken 2830672 times.
✓ Branch 3 taken 11 times.
✓ Branch 4 taken 9 times.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 11 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
5661380 | static Factorials_t factors = binomials<NbCoeffs>::factorials(); |
139 |
2/4✓ Branch 1 taken 2830543 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2830553 times.
✗ Branch 5 not taken.
|
5661384 | Coeffs_t powersOfT, powersOfOneMinusT; |
140 |
1/2✓ Branch 1 taken 2830539 times.
✗ Branch 2 not taken.
|
5661106 | powersOfT(0) = 1; |
141 |
1/2✓ Branch 1 taken 2830553 times.
✗ Branch 2 not taken.
|
5661078 | powersOfOneMinusT(0) = 1; |
142 | 5661106 | const value_type oneMinusT = 1 - t; | |
143 |
2/2✓ Branch 0 taken 8495620 times.
✓ Branch 1 taken 2830572 times.
|
22652384 | for (size_type i = 1; i < NbCoeffs; ++i) { |
144 |
2/4✓ Branch 1 taken 8495609 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8495624 times.
✗ Branch 5 not taken.
|
16991240 | powersOfT(i) = powersOfT(i - 1) * t; |
145 |
2/4✓ Branch 1 taken 8495645 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8495639 times.
✗ Branch 5 not taken.
|
16991248 | powersOfOneMinusT(i) = powersOfOneMinusT(i - 1) * oneMinusT; |
146 | } | ||
147 | |||
148 |
2/2✓ Branch 0 taken 11324710 times.
✓ Branch 1 taken 2822371 times.
|
28294162 | for (size_type i = 0; i < NbCoeffs; ++i) { |
149 | 22649420 | for (size_type p = std::max((size_type)0, k + i - Degree); | |
150 |
2/2✓ Branch 1 taken 13279096 times.
✓ Branch 2 taken 11316509 times.
|
49200796 | p <= std::min(i, k); ++p) { |
151 | 26558192 | size_type ip = i - p; | |
152 |
2/2✓ Branch 0 taken 10353155 times.
✓ Branch 1 taken 2925941 times.
|
53103900 | res(i) += value_type(((k - p) % 2 == 0 ? 1 : -1) * |
153 |
1/2✓ Branch 1 taken 13275199 times.
✗ Branch 2 not taken.
|
26558192 | binomials<NbCoeffs>::binomial(k, p)) * |
154 |
2/4✓ Branch 1 taken 13272288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13272661 times.
✗ Branch 5 not taken.
|
26550398 | powersOfT(ip) * powersOfOneMinusT(Degree - k - ip) / |
155 |
3/6✓ Branch 1 taken 13272706 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13274642 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13272854 times.
✗ Branch 8 not taken.
|
26545322 | value_type(factors(ip) * factors(Degree - k - ip)); |
156 | } | ||
157 | } | ||
158 |
2/4✓ Branch 1 taken 2830606 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2830424 times.
✗ Branch 5 not taken.
|
5644742 | res *= value_type(factors(Degree)); |
159 | } | ||
160 | template <int Degree> | ||
161 | 666 | void spline_basis_function<BernsteinBasis, Degree>::integral( | |
162 | const size_type k, IntegralCoeffs_t& res) { | ||
163 |
1/2✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
|
666 | res.setZero(); |
164 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 332 times.
|
666 | if (k > Degree) return; |
165 | 664 | const int N = 2 * NbCoeffs - 1; | |
166 |
3/6✓ Branch 0 taken 7 times.
✓ Branch 1 taken 325 times.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
664 | static const typename binomials<N>::Factorials_t& factors = |
167 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | binomials<2 * NbCoeffs - 1>::factorials(); |
168 | |||
169 |
2/4✓ Branch 1 taken 332 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 332 times.
✗ Branch 5 not taken.
|
664 | Factorials_t among_k, among_n_minus_k; |
170 |
4/6✓ Branch 1 taken 826 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 826 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 826 times.
✓ Branch 7 taken 332 times.
|
2316 | for (size_type i = 0; i <= k; ++i) among_k(i) = binomials<N>::binomial(k, i); |
171 |
2/2✓ Branch 0 taken 1154 times.
✓ Branch 1 taken 332 times.
|
2972 | for (size_type i = 0; i <= Degree - k; ++i) |
172 |
2/4✓ Branch 1 taken 1154 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1154 times.
✗ Branch 5 not taken.
|
2308 | among_n_minus_k(i) = binomials<N>::binomial(Degree - k, i); |
173 | |||
174 |
2/2✓ Branch 0 taken 1648 times.
✓ Branch 1 taken 332 times.
|
3960 | for (size_type i = 0; i < NbCoeffs; ++i) { |
175 | 3296 | size_type I_i_min = std::max((size_type)0, k + i - Degree); | |
176 | 3296 | size_type I_i_max = std::min(i, k); | |
177 | |||
178 |
2/2✓ Branch 0 taken 9408 times.
✓ Branch 1 taken 1648 times.
|
22112 | for (size_type j = 0; j < NbCoeffs; ++j) { |
179 | 18816 | size_type I_j_min = std::max((size_type)0, k + j - Degree); | |
180 | 18816 | size_type I_j_max = std::min(j, k); | |
181 | |||
182 |
2/2✓ Branch 0 taken 17248 times.
✓ Branch 1 taken 9408 times.
|
53312 | for (size_type p = I_i_min; p <= I_i_max; ++p) { |
183 |
2/2✓ Branch 0 taken 32976 times.
✓ Branch 1 taken 17248 times.
|
100448 | for (size_type q = I_j_min; q <= I_j_max; ++q) { |
184 | 65952 | value_type alpha_0 = | |
185 |
2/4✓ Branch 1 taken 32976 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32976 times.
✗ Branch 5 not taken.
|
65952 | value_type(among_n_minus_k(i - p) * among_n_minus_k(j - q)) / |
186 | 65952 | value_type( | |
187 |
1/2✓ Branch 1 taken 32976 times.
✗ Branch 2 not taken.
|
65952 | binomials<N>::binomial(2 * (Degree - k), i - p + j - q) * |
188 | 65952 | (2 * (Degree - k) + 1)); | |
189 | |||
190 |
2/2✓ Branch 0 taken 16992 times.
✓ Branch 1 taken 15984 times.
|
131904 | res(i, j) += value_type(((2 * k - p - q) % 2 == 0 ? 1 : -1) * |
191 |
3/6✓ Branch 1 taken 32976 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32976 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32976 times.
✗ Branch 8 not taken.
|
65952 | among_k(p) * among_k(q)) * |
192 | alpha_0; | ||
193 | } | ||
194 | } | ||
195 | } | ||
196 | } | ||
197 |
3/6✓ Branch 1 taken 332 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 332 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 332 times.
✗ Branch 8 not taken.
|
664 | res *= value_type(factors(Degree) / factors(Degree - k)); |
198 | } | ||
199 | template <int Degree> | ||
200 | ✗ | void spline_basis_function<BernsteinBasis, Degree>::absDerBounds( | |
201 | Coeffs_t& res) { | ||
202 | ✗ | res.setConstant(2 * Degree); | |
203 | } | ||
204 | template <int Degree> | ||
205 | 45910 | void spline_basis_function<BernsteinBasis, Degree>::bound( | |
206 | const size_type& order, const value_type& t0, const value_type& t1, | ||
207 | Coeffs_t& res) { | ||
208 | (void)order; // Suppress unused warning when NDEBUG | ||
209 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 22955 times.
|
45910 | assert(order == 1); |
210 |
2/4✓ Branch 0 taken 22959 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 22960 times.
✗ Branch 3 not taken.
|
45910 | assert(0 <= t0 && t0 <= 1); |
211 |
2/4✓ Branch 0 taken 22960 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 22964 times.
✗ Branch 3 not taken.
|
45920 | assert(0 <= t1 && t1 <= 1); |
212 |
4/8✓ Branch 0 taken 5 times.
✓ Branch 1 taken 22959 times.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
45928 | static const Coeffs_t b_up = absBound(true); |
213 |
4/8✓ Branch 0 taken 5 times.
✓ Branch 1 taken 22959 times.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
45928 | static const Coeffs_t b_um = absBound(false); |
214 |
2/4✓ Branch 1 taken 22981 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22947 times.
✗ Branch 5 not taken.
|
45928 | Coeffs_t bt0, bt1; |
215 |
1/2✓ Branch 1 taken 22907 times.
✗ Branch 2 not taken.
|
45894 | derivative(1, t0, bt0); |
216 |
1/2✓ Branch 1 taken 22936 times.
✗ Branch 2 not taken.
|
45814 | derivative(1, t1, bt1); |
217 | |||
218 |
5/10✓ Branch 1 taken 22907 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22905 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22918 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22894 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 22912 times.
✗ Branch 14 not taken.
|
45872 | res.noalias() = bt0.cwiseAbs().cwiseMax(bt1.cwiseAbs()); |
219 | |||
220 | // Case i = 0 and i = n | ||
221 | // Nothing to do. | ||
222 | |||
223 | // Case i = 1, n-1 | ||
224 |
4/4✓ Branch 0 taken 13380 times.
✓ Branch 1 taken 9532 times.
✓ Branch 2 taken 2993 times.
✓ Branch 3 taken 10387 times.
|
45824 | if (t0 * Degree < 2 && 2 < t1 * Degree) // If max for i = 1 in [t0, t1] |
225 |
3/6✓ Branch 1 taken 2988 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2985 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2987 times.
✗ Branch 9 not taken.
|
5986 | res(1) = std::max(res(1), b_up(1)); |
226 |
2/2✓ Branch 0 taken 7434 times.
✓ Branch 1 taken 15472 times.
|
45812 | if (t0 * Degree < Degree - 2 && |
227 |
2/2✓ Branch 0 taken 2898 times.
✓ Branch 1 taken 4536 times.
|
14868 | Degree - 2 < t1 * Degree) // If max for i = n-1 in [t0, t1] |
228 |
3/6✓ Branch 1 taken 2900 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2901 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2900 times.
✗ Branch 9 not taken.
|
5796 | res(Degree - 1) = std::max(res(Degree - 1), b_up(Degree - 1)); |
229 | |||
230 | // Case 2 <= i <= n-2 | ||
231 | if (Degree > 3) { | ||
232 | // when u_p(i) in [t0, t1], consider b_up. | ||
233 | 2168 | size_type r1 = std::max(size_type(std::ceil(t0 * (Degree - 1))), | |
234 | 1084 | size_type(2)), | |
235 | 1084 | r2 = size_type(std::floor(t1 * (Degree - 1))), | |
236 | 1084 | nr = std::max(r2 - r1 + 1, size_type(0)); | |
237 |
4/8✓ Branch 1 taken 542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 542 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 542 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 542 times.
✗ Branch 11 not taken.
|
1084 | res.segment(r1, nr).noalias() = |
238 |
2/4✓ Branch 1 taken 542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 542 times.
✗ Branch 5 not taken.
|
1084 | res.segment(r1, nr).cwiseMax(b_up.segment(r1, nr)); |
239 | |||
240 | // when u_m(i) in [t0, t1], consider b_um. | ||
241 | 1084 | r1 = size_type(std::ceil(1 + t0 * (Degree - 1))), | |
242 | 2168 | r2 = std::min(size_type(std::floor(1 + t1 * (Degree - 1))), | |
243 | 1084 | size_type(Degree - 2)); | |
244 | 1084 | nr = std::max(r2 - r1 + 1, size_type(0)); | |
245 |
4/8✓ Branch 1 taken 542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 542 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 542 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 542 times.
✗ Branch 11 not taken.
|
1084 | res.segment(r1, nr).noalias() = |
246 |
2/4✓ Branch 1 taken 542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 542 times.
✗ Branch 5 not taken.
|
1084 | res.segment(r1, nr).cwiseMax(b_um.segment(r1, nr)); |
247 | } | ||
248 | } | ||
249 | template <int Degree> | ||
250 | typename spline_basis_function<BernsteinBasis, Degree>::Coeffs_t | ||
251 | 20 | spline_basis_function<BernsteinBasis, Degree>::absBound(bool up) { | |
252 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | Coeffs_t res; |
253 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | Factorials_t iToThePowerOfI(Factorials_t::Ones()); |
254 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 10 times.
|
60 | for (size_type i = 1; i < Degree; ++i) { |
255 |
3/4✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 38 times.
✓ Branch 4 taken 20 times.
|
116 | for (size_type j = 0; j < i; ++j) iToThePowerOfI(i) *= i; |
256 | } | ||
257 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | res(0) = res(Degree) = Degree; |
258 | if (Degree > 1) { | ||
259 | if (Degree == 2) | ||
260 | res(1) = res(Degree - 1) = 2; | ||
261 | else | ||
262 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | res(1) = res(Degree - 1) = |
263 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
16 | value_type(iToThePowerOfI(Degree - 2)) / std::pow(Degree, Degree - 3); |
264 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 8 times.
|
24 | for (size_type i = 2; i < Degree - 1; ++i) { |
265 | 8 | const size_type p = | |
266 |
4/6✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
8 | (up ? iToThePowerOfI(i) * iToThePowerOfI(Degree - i - 1) |
267 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | : iToThePowerOfI(Degree - i) * iToThePowerOfI(i - 1)); |
268 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
16 | res(i) = value_type(binomials<NbCoeffs>::binomial(Degree, i) * p) / |
269 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | value_type(iToThePowerOfI(Degree - 1)); |
270 | } | ||
271 | } | ||
272 | 40 | return res; | |
273 | } | ||
274 | } // namespace internal | ||
275 | |||
276 | template <int _SplineType, int _Order> | ||
277 | ✗ | std::ostream& Spline<_SplineType, _Order>::print(std::ostream& os) const { | |
278 | ✗ | os << "Spline (type=" << PolynomeBasis << ", order=" << Order | |
279 | ✗ | << ")\nbase = " << base().transpose() << '\n' | |
280 | ✗ | << parameters_ << std::endl; | |
281 | ✗ | return os; | |
282 | } | ||
283 | |||
284 | template <int _SplineType, int _Order> | ||
285 | 3705580 | Spline<_SplineType, _Order>::Spline(const Spline& path) | |
286 | : Path(path), | ||
287 | 3705580 | parameterSize_(path.parameterSize_), | |
288 | 3705580 | robot_(path.robot_), | |
289 |
1/2✓ Branch 1 taken 1852790 times.
✗ Branch 2 not taken.
|
3705580 | base_(path.base_), |
290 |
1/2✓ Branch 1 taken 1852790 times.
✗ Branch 2 not taken.
|
3705580 | parameters_(path.parameters_), |
291 |
1/2✓ Branch 1 taken 1852790 times.
✗ Branch 2 not taken.
|
3705580 | velocity_(path.velocity_), |
292 |
1/2✓ Branch 4 taken 1852790 times.
✗ Branch 5 not taken.
|
11116740 | powersOfT_(path.powersOfT_) {} |
293 | |||
294 | template <int _SplineType, int _Order> | ||
295 | 48 | Spline<_SplineType, _Order>::Spline(const Spline& path, | |
296 | const ConstraintSetPtr_t& constraints) | ||
297 | : Path(path, constraints), | ||
298 | 48 | parameterSize_(path.parameterSize_), | |
299 | 48 | robot_(path.robot_), | |
300 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
48 | base_(path.base_), |
301 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
48 | parameters_(path.parameters_), |
302 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
48 | velocity_(path.velocity_), |
303 |
1/2✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
144 | powersOfT_(path.powersOfT_) {} |
304 | |||
305 | template <int _SplineType, int _Order> | ||
306 | 6 | void Spline<_SplineType, _Order>::timeFreeBasisFunctionDerivative( | |
307 | const size_type order, const value_type& u, BasisFunctionVector_t& res) { | ||
308 | // TODO: add a cache. | ||
309 |
2/4✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | assert(u >= 0 && u <= 1); |
310 | 6 | BasisFunction_t::derivative(order, u, res); | |
311 | } | ||
312 | |||
313 | template <int _SplineType, int _Order> | ||
314 | 1859062 | void Spline<_SplineType, _Order>::basisFunctionDerivative( | |
315 | const size_type order, const value_type& u, | ||
316 | BasisFunctionVector_t& res) const { | ||
317 | // TODO: add a cache. | ||
318 |
2/4✓ Branch 0 taken 929531 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 929531 times.
✗ Branch 3 not taken.
|
1859062 | assert(u >= 0 && u <= 1); |
319 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 929531 times.
|
1859062 | if (length() == 0) |
320 | ✗ | res.setZero(); | |
321 | else { | ||
322 | 1859062 | BasisFunction_t::derivative(order, u, res); | |
323 | 1859068 | res /= powersOfT_(order); | |
324 | } | ||
325 | 1859062 | } | |
326 | |||
327 | template <int _SplineType, int _Order> | ||
328 | ✗ | void Spline<_SplineType, _Order>::maxVelocity(vectorOut_t res) const { | |
329 | ✗ | BasisFunctionVector_t ub; | |
330 | ✗ | BasisFunction_t::absDerBounds(ub); | |
331 | ✗ | ub /= length(); | |
332 | ✗ | res.transpose() = ub.transpose() * parameters_.cwiseAbs(); | |
333 | } | ||
334 | |||
335 | template <int _SplineType, int _Order> | ||
336 | 666 | void Spline<_SplineType, _Order>::squaredNormBasisFunctionIntegral( | |
337 | const size_type order, BasisFunctionIntegralMatrix_t& Ic) const { | ||
338 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 333 times.
|
666 | assert(std::max(size_type(1), 2 * order - 1) < powersOfT_.size()); |
339 | |||
340 | // TODO: add a cache. | ||
341 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 333 times.
|
666 | if (length() == 0) |
342 | ✗ | Ic.setZero(); | |
343 | else { | ||
344 | 666 | BasisFunction_t::integral(order, Ic); | |
345 |
1/2✓ Branch 0 taken 333 times.
✗ Branch 1 not taken.
|
666 | if (order > 0) |
346 | 666 | Ic /= powersOfT_[2 * order - 1]; | |
347 | else | ||
348 | ✗ | Ic *= powersOfT_[1]; | |
349 | } | ||
350 | 666 | } | |
351 | |||
352 | template <int _SplineType, int _Order> | ||
353 | 3710410 | bool Spline<_SplineType, _Order>::impl_compute(ConfigurationOut_t res, | |
354 | value_type s) const { | ||
355 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1855205 times.
|
3710410 | if (paramLength() == 0) |
356 | ✗ | value(base_, parameters_, 0, res, velocity_); | |
357 | else { | ||
358 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1855205 times.
|
3710410 | assert(s >= paramRange().first - |
359 | Eigen::NumTraits<value_type>::dummy_precision()); | ||
360 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1855205 times.
|
3710410 | assert(s <= paramRange().second + |
361 | Eigen::NumTraits<value_type>::dummy_precision()); | ||
362 | |||
363 | 3710410 | value_type u = (s - paramRange().first) / paramLength(); | |
364 | // clamp u between 0 and 1. | ||
365 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1855205 times.
|
3710410 | if (u < 0.) |
366 | ✗ | u = 0.; | |
367 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1855205 times.
|
3710410 | else if (u > 1.) |
368 | ✗ | u = 1.; | |
369 |
5/10✓ Branch 1 taken 1855205 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1855205 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1855205 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1855205 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1855204 times.
✗ Branch 14 not taken.
|
3710410 | value(base_, parameters_, u, res, velocity_); |
370 | } | ||
371 | 3710410 | return true; | |
372 | } | ||
373 | |||
374 | template <int _SplineType, int _Order> | ||
375 | 1855972 | void Spline<_SplineType, _Order>::impl_derivative(vectorOut_t res, | |
376 | const value_type& s, | ||
377 | size_type order) const { | ||
378 | // p = q + v(t) so dp/dt = d+/dv * dv/dt | ||
379 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 927986 times.
|
1855972 | assert(order > 0); |
380 | // For non vector space, it is not possible to compute the derivatives | ||
381 | // at a higher order. At the d2+/dv2 is not available for SE(n) and SO(n). | ||
382 |
4/6✓ Branch 0 taken 362 times.
✓ Branch 1 taken 927624 times.
✓ Branch 6 taken 362 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 362 times.
|
1855972 | assert(order == 1 || robot_->configSpace()->isVectorSpace()); |
383 |
1/2✓ Branch 1 taken 927986 times.
✗ Branch 2 not taken.
|
1855972 | BasisFunctionVector_t basisFunc; |
384 | 1855972 | const value_type u = | |
385 |
2/4✓ Branch 1 taken 927986 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 927986 times.
✗ Branch 4 not taken.
|
1855972 | (length() == 0 ? 0 : (s - paramRange().first) / paramLength()); |
386 |
1/2✓ Branch 1 taken 927986 times.
✗ Branch 2 not taken.
|
1855972 | basisFunctionDerivative(order, u, basisFunc); |
387 |
4/8✓ Branch 1 taken 927986 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 927986 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 927986 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 927986 times.
✗ Branch 11 not taken.
|
1855972 | res.noalias() = parameters_.transpose() * basisFunc; |
388 | |||
389 |
3/4✓ Branch 4 taken 927986 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 975 times.
✓ Branch 7 taken 927011 times.
|
1855972 | if (!robot_->configSpace()->isVectorSpace()) { |
390 |
1/2✓ Branch 1 taken 975 times.
✗ Branch 2 not taken.
|
1950 | basisFunctionDerivative(0, u, basisFunc); |
391 |
3/6✓ Branch 1 taken 975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 975 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 975 times.
✗ Branch 8 not taken.
|
1950 | vector_t v(parameters_.transpose() * basisFunc); |
392 | // true means: res <- Jdiff * res | ||
393 |
4/8✓ Branch 3 taken 975 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 975 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 975 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 975 times.
✗ Branch 13 not taken.
|
1950 | base_.space()->dIntegrate_dv<pinocchio::DerivativeTimesInput>(base_, v, |
394 | res); | ||
395 | } | ||
396 | 1855972 | } | |
397 | |||
398 | template <int _SplineType, int _Order> | ||
399 | ✗ | void Spline<_SplineType, _Order>::impl_paramDerivative( | |
400 | vectorOut_t res, const value_type& s) const { | ||
401 | ✗ | BasisFunctionVector_t basisFunc; | |
402 | ✗ | const value_type u = | |
403 | ✗ | (length() == 0 ? 0 : (s - paramRange().first) / paramLength()); | |
404 | ✗ | basisFunctionDerivative(0, u, basisFunc); | |
405 | ✗ | res = basisFunc; | |
406 | |||
407 | ✗ | if (!robot_->configSpace()->isVectorSpace()) { | |
408 | ✗ | vector_t v(parameters_.transpose() * basisFunc); | |
409 | ✗ | matrix_t unused; | |
410 | // true means: res <- Jdiff * res | ||
411 | ✗ | base_.space()->Jdifference<true>(base(), v, unused, res); | |
412 | } | ||
413 | } | ||
414 | |||
415 | template <int _SplineType, int _Order> | ||
416 | ✗ | void Spline<_SplineType, _Order>::impl_paramIntegrate(vectorIn_t dParam) { | |
417 | // pinocchio::integrate<false, hpp::pinocchio::LieGroupTpl> | ||
418 | // (robot_, base_, dParam.head(robot_->numberDof()), base_); | ||
419 | |||
420 | ✗ | ParameterVector_t(parameters_.data(), parameters_.size()).noalias() += dParam; | |
421 | } | ||
422 | |||
423 | template <int _SplineType, int _Order> | ||
424 | 46002 | void Spline<_SplineType, _Order>::impl_velocityBound( | |
425 | vectorOut_t res, const value_type& t0, const value_type& t1) const { | ||
426 |
1/2✓ Branch 1 taken 22955 times.
✗ Branch 2 not taken.
|
46002 | BasisFunctionVector_t ub; |
427 | 45888 | const value_type u0 = | |
428 |
2/4✓ Branch 1 taken 22950 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 22950 times.
✗ Branch 4 not taken.
|
45910 | (length() == 0 ? 0 : (t0 - paramRange().first) / paramLength()); |
429 | 45900 | const value_type u1 = | |
430 |
2/4✓ Branch 1 taken 22951 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 22954 times.
✗ Branch 4 not taken.
|
45888 | (length() == 0 ? 0 : (t1 - paramRange().first) / paramLength()); |
431 |
1/2✓ Branch 1 taken 22915 times.
✗ Branch 2 not taken.
|
45900 | BasisFunction_t::bound(1, u0, u1, ub); |
432 |
1/2✓ Branch 2 taken 22942 times.
✗ Branch 3 not taken.
|
45830 | ub /= paramLength(); |
433 |
5/10✓ Branch 1 taken 22927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22914 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22902 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22872 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 22942 times.
✗ Branch 14 not taken.
|
45884 | res.noalias() = parameters_.cwiseAbs().transpose() * ub; |
434 | } | ||
435 | |||
436 | template <int _SplineType, int _Order> | ||
437 | 562 | value_type Spline<_SplineType, _Order>::squaredNormIntegral( | |
438 | const size_type order) const { | ||
439 |
1/2✓ Branch 1 taken 281 times.
✗ Branch 2 not taken.
|
562 | typename sbf_traits::IntegralCoeffs_t Ic; |
440 |
1/2✓ Branch 1 taken 281 times.
✗ Branch 2 not taken.
|
562 | squaredNormBasisFunctionIntegral(order, Ic); |
441 |
4/8✓ Branch 1 taken 281 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 281 times.
✗ Branch 11 not taken.
|
562 | return (parameters_ * parameters_.transpose()).cwiseProduct(Ic).sum(); |
442 | // return (parameters_.transpose() * (Ic * parameters_)).trace(); | ||
443 | } | ||
444 | |||
445 | template <int _SplineType, int _Order> | ||
446 | 2 | void Spline<_SplineType, _Order>::squaredNormIntegralDerivative( | |
447 | const size_type order, vectorOut_t res) const { | ||
448 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | typename BasisFunction_t::IntegralCoeffs_t Ic; |
449 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | squaredNormBasisFunctionIntegral(order, Ic); |
450 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | matrix_t tmp(parameters_.transpose() * Ic); |
451 |
4/8✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | res = 2 * Eigen::Map<vector_t, Eigen::Aligned>(tmp.data(), tmp.size()); |
452 | } | ||
453 | |||
454 | template <int _SplineType, int _Order> | ||
455 | 3710410 | void Spline<_SplineType, _Order>::value( | |
456 | pinocchio::LiegroupElementConstRef base, | ||
457 | Eigen::Ref<const ParameterMatrix_t> params, const value_type& u, | ||
458 | ConfigurationOut_t res, vectorOut_t velocity) { | ||
459 |
2/4✓ Branch 0 taken 1855205 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1855205 times.
✗ Branch 3 not taken.
|
3710410 | assert(0 <= u && u <= 1); |
460 | assert(params.rows() == NbCoeffs); | ||
461 |
1/2✗ Branch 4 not taken.
✓ Branch 5 taken 1855205 times.
|
3710410 | assert(params.cols() == base.space()->nv()); |
462 | |||
463 | 3710410 | velocity.resize(base.space()->nv()); | |
464 | |||
465 |
1/2✓ Branch 1 taken 1855204 times.
✗ Branch 2 not taken.
|
3710410 | BasisFunctionVector_t basisFunc; |
466 |
1/2✓ Branch 1 taken 1855204 times.
✗ Branch 2 not taken.
|
3710408 | BasisFunction_t::derivative(0, u, basisFunc); |
467 |
4/8✓ Branch 1 taken 1855204 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1855205 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1855204 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1855205 times.
✗ Branch 11 not taken.
|
3710408 | velocity.noalias() = params.transpose() * basisFunc; |
468 | |||
469 |
3/6✓ Branch 1 taken 1855205 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1855204 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1855205 times.
✗ Branch 9 not taken.
|
3710410 | res = (base + velocity).vector(); |
470 | 3710410 | } | |
471 | |||
472 | template <int _SplineType, int _Order> | ||
473 | template <class Archive> | ||
474 | ✗ | void Spline<_SplineType, _Order>::serialize(Archive& ar, | |
475 | const unsigned int version) { | ||
476 | using namespace boost::serialization; | ||
477 | (void)version; | ||
478 | ✗ | ar& make_nvp("base", base_object<Path>(*this)); | |
479 | ✗ | ar& BOOST_SERIALIZATION_NVP(parameterSize_); | |
480 | ✗ | ar& BOOST_SERIALIZATION_NVP(robot_); | |
481 | ✗ | ar& BOOST_SERIALIZATION_NVP(base_); | |
482 | ✗ | ar& BOOST_SERIALIZATION_NVP(parameters_); | |
483 | ✗ | ar& BOOST_SERIALIZATION_NVP(velocity_); | |
484 | ✗ | ar& BOOST_SERIALIZATION_NVP(powersOfT_); | |
485 | ✗ | ar& BOOST_SERIALIZATION_NVP(weak_); | |
486 | } | ||
487 | |||
488 | // template class Spline<CanonicalPolynomeBasis, 1>; // equivalent to | ||
489 | // StraightPath template class Spline<CanonicalPolynomeBasis, 2>; template class | ||
490 | // Spline<CanonicalPolynomeBasis, 3>; | ||
491 | template class Spline<BernsteinBasis, 1>; // equivalent to StraightPath | ||
492 | // template class Spline<BernsteinBasis, 2>; | ||
493 | template class Spline<BernsteinBasis, 3>; | ||
494 | template class Spline<BernsteinBasis, 5>; | ||
495 | template class Spline<BernsteinBasis, 7>; | ||
496 | typedef Spline<BernsteinBasis, 1> | ||
497 | Spline_BernsteinBasis_1; // equivalent to StraightPath | ||
498 | typedef Spline<BernsteinBasis, 3> Spline_BernsteinBasis_3; | ||
499 | typedef Spline<BernsteinBasis, 5> Spline_BernsteinBasis_5; | ||
500 | typedef Spline<BernsteinBasis, 7> Spline_BernsteinBasis_7; | ||
501 | |||
502 | HPP_SERIALIZATION_IMPLEMENT(Spline_BernsteinBasis_1); | ||
503 | HPP_SERIALIZATION_IMPLEMENT(Spline_BernsteinBasis_3); | ||
504 | HPP_SERIALIZATION_IMPLEMENT(Spline_BernsteinBasis_5); | ||
505 | HPP_SERIALIZATION_IMPLEMENT(Spline_BernsteinBasis_7); | ||
506 | } // namespace path | ||
507 | } // namespace core | ||
508 | } // namespace hpp | ||
509 | |||
510 | 18 | BOOST_CLASS_EXPORT(hpp::core::path::Spline_BernsteinBasis_1) | |
511 | 18 | BOOST_CLASS_EXPORT(hpp::core::path::Spline_BernsteinBasis_3) | |
512 | 18 | BOOST_CLASS_EXPORT(hpp::core::path::Spline_BernsteinBasis_5) | |
513 | 18 | BOOST_CLASS_EXPORT(hpp::core::path::Spline_BernsteinBasis_7) | |
514 |