GCC Code Coverage Report

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