GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/cholesky.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 135 176 76.7%
Branches: 109 473 23.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_cholesky_hxx__
6 #define __pinocchio_cholesky_hxx__
7
8 #include "pinocchio/algorithm/check.hpp"
9
10 /// @cond DEV
11
12 namespace pinocchio
13 {
14 namespace cholesky
15 {
16
17 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
18 225 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & decompose(
19 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
20 DataTpl<Scalar, Options, JointCollectionTpl> & data)
21 {
22 /*
23 * D = zeros(n,1);
24 * U = eye(n);
25 * for j=n:-1:1
26 * subtree = j+1:tree(j);
27 * D(j) = M(j,j) - U(j,subtree)*diag(D(subtree))*U(j,subtree)';
28 * i=parent(j);
29 * while i>0
30 * U(i,j) = (M(i,j) - U(i,subtree)*diag(D(subtree))*U(j,subtree)') / D(j);
31 * i=parent(i);
32 * end
33 * end
34 */
35 PINOCCHIO_UNUSED_VARIABLE(model);
36
1/2
✓ Branch 1 taken 225 times.
✗ Branch 2 not taken.
 225 assert(model.check(data) && "data is not consistent with model.");
37
38 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
39
40 225 const typename Data::MatrixXs & M = data.M;
41 225 typename Data::MatrixXs & U = data.U;
42 225 typename Data::VectorXs & D = data.D;
43 225 typename Data::VectorXs & Dinv = data.Dinv;
44
45
2/2
✓ Branch 0 taken 7186 times.
✓ Branch 1 taken 225 times.
 7411 for (int j = model.nv - 1; j >= 0; --j)
46 {
47 7186 const int NVT = data.nvSubtree_fromRow[(size_t)j] - 1;
48
1/2
✓ Branch 1 taken 7186 times.
✗ Branch 2 not taken.
 7186 typename Data::VectorXs::SegmentReturnType DUt = data.tmp.head(NVT);
49
2/2
✓ Branch 0 taken 6286 times.
✓ Branch 1 taken 900 times.
 7186 if (NVT)
50
3/6
✓ Branch 1 taken 6286 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6286 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6286 times.
✗ Branch 8 not taken.
 6286 DUt.noalias() =
51
4/8
✓ Branch 1 taken 6286 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6286 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6286 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6286 times.
✗ Branch 11 not taken.
 12572 U.row(j).segment(j + 1, NVT).transpose().cwiseProduct(D.segment(j + 1, NVT));
52
53
5/10
✓ Branch 1 taken 7186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7186 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7186 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7186 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7186 times.
✗ Branch 14 not taken.
 7186 D[j] = M(j, j) - U.row(j).segment(j + 1, NVT).dot(DUt);
54
2/4
✓ Branch 1 taken 7186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7186 times.
✗ Branch 5 not taken.
 7186 Dinv[j] = Scalar(1) / D[j];
55
2/2
✓ Branch 1 taken 57443 times.
✓ Branch 2 taken 7186 times.
 64629 for (int _i = data.parents_fromRow[(size_t)j]; _i >= 0;
56 57443 _i = data.parents_fromRow[(size_t)_i])
57
6/12
✓ Branch 1 taken 57443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 57443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 57443 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 57443 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 57443 times.
✗ Branch 17 not taken.
 57443 U(_i, j) = (M(_i, j) - U.row(_i).segment(j + 1, NVT).dot(DUt)) * Dinv[j];
58 }
59
60 225 return data.U;
61 }
62
63 namespace internal
64 {
65 template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
66 struct Uv
67 {
68 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
69 1 static void run(
70 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
71 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
72 const Eigen::MatrixBase<Mat> & m)
73 {
74 1 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
75
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 1 times.
 21 for (int k = 0; k < m_.cols(); ++k)
76
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 cholesky::Uv(model, data, m_.col(k));
77 1 }
78 };
79
80 template<typename Mat>
81 struct Uv<Mat, 1>
82 {
83 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
84 20 static void run(
85 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
86 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
87 const Eigen::MatrixBase<Mat> & v)
88 {
89 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
90
91 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
92 PINOCCHIO_UNUSED_VARIABLE(model);
93
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 assert(model.check(data) && "data is not consistent with model.");
94
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 20 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
95
96 20 Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v);
97
98 20 const typename Data::MatrixXs & U = data.U;
99 20 const std::vector<int> & nvt = data.nvSubtree_fromRow;
100
101
2/2
✓ Branch 0 taken 620 times.
✓ Branch 1 taken 20 times.
 640 for (int k = 0; k < model.nv - 1; ++k) // You can stop one step before nv
102
4/8
✓ Branch 1 taken 620 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 620 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 620 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 620 times.
✗ Branch 12 not taken.
 1240 v_.row(k) += U.row(k).segment(k + 1, nvt[(size_t)k] - 1)
103
1/2
✓ Branch 3 taken 620 times.
✗ Branch 4 not taken.
 1860 * v_.middleRows(k + 1, nvt[(size_t)k] - 1);
104 20 }
105 };
106
107 } // namespace internal
108
109 /* Compute U*v.
110 * Nota: there is no smart way of doing U*D*v, so it is not proposed. */
111 template<
112 typename Scalar,
113 int Options,
114 template<typename, int>
115 class JointCollectionTpl,
116 typename Mat>
117 42 Mat & Uv(
118 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
119 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
120 const Eigen::MatrixBase<Mat> & m)
121 {
122 42 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
123 42 internal::Uv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_);
124 42 return m_.derived();
125 }
126
127 namespace internal
128 {
129 template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
130 struct Utv
131 {
132 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
133 1 static void run(
134 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
135 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
136 const Eigen::MatrixBase<Mat> & m)
137 {
138 1 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
139
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 1 times.
 21 for (int k = 0; k < m_.cols(); ++k)
140
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 cholesky::Utv(model, data, m_.col(k));
141 1 }
142 };
143
144 template<typename Mat>
145 struct Utv<Mat, 1>
146 {
147 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
148 20 static void run(
149 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
150 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
151 const Eigen::MatrixBase<Mat> & v)
152 {
153 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
154
155 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
156
157 PINOCCHIO_UNUSED_VARIABLE(model);
158
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 assert(model.check(data) && "data is not consistent with model.");
159
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 20 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
160 20 Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v);
161
162 20 const typename Data::MatrixXs & U = data.U;
163 20 const std::vector<int> & nvt = data.nvSubtree_fromRow;
164
165
2/2
✓ Branch 0 taken 620 times.
✓ Branch 1 taken 20 times.
 640 for (int k = model.nv - 2; k >= 0; --k) // You can start from nv-2 (no child in nv-1)
166
3/6
✓ Branch 1 taken 620 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 620 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 620 times.
✗ Branch 9 not taken.
 620 v_.segment(k + 1, nvt[(size_t)k] - 1) +=
167
2/4
✓ Branch 4 taken 620 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 620 times.
✗ Branch 8 not taken.
 1240 U.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * v_[k];
168 20 }
169 };
170
171 } // namespace internal
172
173 /* Compute U'*v */
174 template<
175 typename Scalar,
176 int Options,
177 template<typename, int>
178 class JointCollectionTpl,
179 typename Mat>
180 42 Mat & Utv(
181 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
182 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
183 const Eigen::MatrixBase<Mat> & m)
184 {
185 42 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
186 42 internal::Utv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_);
187 42 return m_.derived();
188 }
189
190 namespace internal
191 {
192 template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
193 struct Uiv
194 {
195 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
196 217 static void run(
197 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
198 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
199 const Eigen::MatrixBase<Mat> & m)
200 {
201 217 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
202
2/2
✓ Branch 1 taken 1439 times.
✓ Branch 2 taken 217 times.
 1656 for (int k = 0; k < m_.cols(); ++k)
203
1/2
✓ Branch 2 taken 1439 times.
✗ Branch 3 not taken.
 1439 cholesky::Uiv(model, data, m_.col(k));
204 217 }
205 };
206
207 template<typename Mat>
208 struct Uiv<Mat, 1>
209 {
210 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
211 1462 static void run(
212 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
213 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
214 const Eigen::MatrixBase<Mat> & v)
215 {
216 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
217
218 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
219
1/2
✓ Branch 1 taken 1460 times.
✗ Branch 2 not taken.
 1462 assert(model.check(data) && "data is not consistent with model.");
220
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1460 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 1462 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
221 1462 Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v);
222
223 1462 const typename Data::MatrixXs & U = data.U;
224 1462 const std::vector<int> & nvt = data.nvSubtree_fromRow;
225
226
2/2
✓ Branch 0 taken 45064 times.
✓ Branch 1 taken 1460 times.
 46560 for (int k = model.nv - 2; k >= 0; --k) // You can start from nv-2 (no child in nv-1)
227 45098 v_[k] -= U.row(k)
228
1/2
✓ Branch 2 taken 45064 times.
✗ Branch 3 not taken.
 45098 .segment(k + 1, nvt[(size_t)k] - 1)
229
3/6
✓ Branch 2 taken 45064 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45064 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45064 times.
✗ Branch 9 not taken.
 45098 .dot(v_.segment(k + 1, nvt[(size_t)k] - 1));
230 1462 }
231 };
232
233 } // namespace internal
234
235 /* Compute U^{-1}*v
236 * Nota: there is no efficient way to compute D^{-1}U^{-1}v
237 * in a single loop, so algorithm is not proposed.*/
238 template<
239 typename Scalar,
240 int Options,
241 template<typename, int>
242 class JointCollectionTpl,
243 typename Mat>
244 3335 Mat & Uiv(
245 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
246 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
247 const Eigen::MatrixBase<Mat> & m)
248 {
249 3335 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
250 3335 internal::Uiv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_);
251 3335 return m_.derived();
252 }
253
254 namespace internal
255 {
256 template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
257 struct Utiv
258 {
259 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
260 1 static void run(
261 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
262 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
263 const Eigen::MatrixBase<Mat> & m)
264 {
265 1 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
266
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 1 times.
 21 for (int k = 0; k < m_.cols(); ++k)
267
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 cholesky::Utiv(model, data, m_.col(k));
268 1 }
269 };
270
271 template<typename Mat>
272 struct Utiv<Mat, 1>
273 {
274 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
275 43 static void run(
276 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
277 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
278 const Eigen::MatrixBase<Mat> & v)
279 {
280 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
281
282 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
283
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 43 assert(model.check(data) && "data is not consistent with model.");
284
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 41 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 43 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
285 43 Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v);
286
287 43 const typename Data::MatrixXs & U = data.U;
288 43 const std::vector<int> & nvt = data.nvSubtree_fromRow;
289
290
2/2
✓ Branch 0 taken 1243 times.
✓ Branch 1 taken 41 times.
 1320 for (int k = 0; k < model.nv - 1; ++k) // You can stop one step before nv.
291
3/6
✓ Branch 1 taken 1243 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1243 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1243 times.
✗ Branch 9 not taken.
 1277 v_.segment(k + 1, nvt[(size_t)k] - 1) -=
292
2/4
✓ Branch 4 taken 1243 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1243 times.
✗ Branch 8 not taken.
 2554 U.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * v_[k];
293 43 }
294 };
295
296 } // namespace internal
297
298 template<
299 typename Scalar,
300 int Options,
301 template<typename, int>
302 class JointCollectionTpl,
303 typename Mat>
304 65 Mat & Utiv(
305 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
306 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
307 const Eigen::MatrixBase<Mat> & m)
308 {
309 65 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
310 65 internal::Utiv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_);
311 65 return m_.derived();
312 }
313
314 namespace internal
315 {
316 template<typename Mat, typename MatRes, int ColsAtCompileTime = Mat::ColsAtCompileTime>
317 struct Mv
318 {
319 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
320 ✗ static void run(
321 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
322 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
323 const Eigen::MatrixBase<Mat> & min,
324 const Eigen::MatrixBase<MatRes> & mout)
325 {
326 ✗ MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes, mout);
327 ✗ for (int k = 0; k < min.cols(); ++k)
328 ✗ cholesky::Mv(model, data, min.col(k), mout_.col(k));
329 }
330 };
331
332 template<typename Mat, typename MatRes>
333 struct Mv<Mat, MatRes, 1>
334 {
335 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
336 ✗ static void run(
337 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
338 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
339 const Eigen::MatrixBase<Mat> & vin,
340 const Eigen::MatrixBase<MatRes> & vout)
341 {
342 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
343
344 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
345 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes)
346
347 PINOCCHIO_UNUSED_VARIABLE(model);
348 ✗ assert(model.check(data) && "data is not consistent with model.");
349 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv);
350 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv);
351
352 ✗ MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes, vout);
353
354 ✗ const typename Data::MatrixXs & M = data.M;
355 ✗ const std::vector<int> & nvt = data.nvSubtree_fromRow;
356
357 ✗ for (int k = model.nv - 1; k >= 0; --k)
358 {
359 ✗ vout_[k] = M.row(k).segment(k, nvt[(size_t)k]) * vin.segment(k, nvt[(size_t)k]);
360 ✗ vout_.segment(k + 1, nvt[(size_t)k] - 1) +=
361 ✗ M.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * vin[k];
362 }
363 }
364 };
365
366 } // namespace internal
367
368 template<
369 typename Scalar,
370 int Options,
371 template<typename, int>
372 class JointCollectionTpl,
373 typename Mat,
374 typename MatRes>
375 ✗ MatRes & Mv(
376 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
377 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
378 const Eigen::MatrixBase<Mat> & min,
379 const Eigen::MatrixBase<MatRes> & mout)
380 {
381 ✗ MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes, mout);
382 ✗ internal::Mv<Mat, MatRes, Mat::ColsAtCompileTime>::run(model, data, min.derived(), mout_);
383 ✗ return mout_.derived();
384 }
385
386 template<
387 typename Scalar,
388 int Options,
389 template<typename, int>
390 class JointCollectionTpl,
391 typename Mat>
392 ✗ typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(
393 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
394 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
395 const Eigen::MatrixBase<Mat> & min)
396 {
397 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) ReturnType;
398 ✗ ReturnType res(model.nv, min.cols());
399 ✗ return Mv(model, data, min, res);
400 }
401
402 namespace internal
403 {
404 template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
405 struct UDUtv
406 {
407 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
408 ✗ static void run(
409 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
410 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
411 const Eigen::MatrixBase<Mat> & m)
412 {
413 ✗ Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
414 ✗ for (int k = 0; k < m_.cols(); ++k)
415 ✗ cholesky::UDUtv(model, data, m_.col(k));
416 }
417 };
418
419 template<typename Mat>
420 struct UDUtv<Mat, 1>
421 {
422 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
423 ✗ static void run(
424 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
425 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
426 const Eigen::MatrixBase<Mat> & v)
427 {
428 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
429
430 ✗ assert(model.check(data) && "data is not consistent with model.");
431 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
432
433 ✗ Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v);
434
435 ✗ cholesky::Utv(model, data, v_);
436 ✗ v_.array() *= data.D.array();
437 ✗ cholesky::Uv(model, data, v_);
438 }
439 };
440
441 } // namespace internal
442
443 template<
444 typename Scalar,
445 int Options,
446 template<typename, int>
447 class JointCollectionTpl,
448 typename Mat>
449 ✗ Mat & UDUtv(
450 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
451 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
452 const Eigen::MatrixBase<Mat> & m)
453 {
454 ✗ Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
455
456 ✗ internal::UDUtv<Mat>::run(model, data, m_);
457 ✗ return m_;
458 }
459
460 namespace internal
461 {
462 template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime>
463 struct solve
464 {
465 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
466 ✗ static void run(
467 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
468 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
469 const Eigen::MatrixBase<Mat> & m)
470 {
471 ✗ Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
472 ✗ for (int k = 0; k < m_.cols(); ++k)
473 ✗ cholesky::solve(model, data, m_.col(k));
474 }
475 };
476
477 template<typename Mat>
478 struct solve<Mat, 1>
479 {
480 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
481 23 static void run(
482 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
483 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
484 const Eigen::MatrixBase<Mat> & v)
485 {
486 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
487
488
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 23 assert(model.check(data) && "data is not consistent with model.");
489
490 23 Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v);
491
492 23 cholesky::Uiv(model, data, v_);
493
2/4
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
 23 v_.array() *= data.Dinv.array();
494 23 cholesky::Utiv(model, data, v_);
495 23 }
496 };
497
498 } // namespace internal
499
500 template<
501 typename Scalar,
502 int Options,
503 template<typename, int>
504 class JointCollectionTpl,
505 typename Mat>
506 21 Mat & solve(
507 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
508 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
509 const Eigen::MatrixBase<Mat> & m)
510 {
511 21 Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m);
512 21 internal::solve<Mat, Mat::ColsAtCompileTime>::run(model, data, m_);
513 21 return m_;
514 }
515
516 namespace internal
517 {
518 template<
519 typename Scalar,
520 int Options,
521 template<typename, int>
522 class JointCollectionTpl,
523 typename VectorLike>
524 64 VectorLike & Miunit(
525 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
526 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
527 const int col,
528 const Eigen::MatrixBase<VectorLike> & v)
529 {
530 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
531
532 PINOCCHIO_UNUSED_VARIABLE(model);
533
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 64 assert(model.check(data) && "data is not consistent with model.");
534
2/6
✓ Branch 0 taken 64 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 64 PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0);
535
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 64 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
536
537 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
538
539 64 const typename Data::MatrixXs & U = data.U;
540 64 const std::vector<int> & nvt = data.nvSubtree_fromRow;
541 64 VectorLike & v_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike, v);
542
543 64 v_[col] = (typename VectorLike::Scalar)1;
544 64 const int last_col =
545 64 std::min<int>(col - 1, model.nv - 2); // You can start from nv-2 (no child in nv-1)
546
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 v_.tail(model.nv - col - 1).setZero();
547
2/2
✓ Branch 0 taken 992 times.
✓ Branch 1 taken 64 times.
 1056 for (int k = last_col; k >= 0; --k)
548 {
549 992 const int nvt_max = std::min<int>(col, nvt[(size_t)k] - 1);
550
4/8
✓ Branch 2 taken 992 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 992 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 992 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 992 times.
✗ Branch 12 not taken.
 992 v_[k] = -U.row(k).segment(k + 1, nvt_max).dot(v_.segment(k + 1, nvt_max));
551 }
552
553
4/8
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 64 times.
✗ Branch 12 not taken.
 64 v_.head(col + 1).array() *= data.Dinv.head(col + 1).array();
554
555
2/2
✓ Branch 0 taken 1056 times.
✓ Branch 1 taken 64 times.
 1120 for (int k = 0; k < col + 1; ++k)
556 {
557 1056 const int nvt_max = nvt[(size_t)k] - 1;
558
5/10
✓ Branch 3 taken 1056 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1056 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1056 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1056 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1056 times.
✗ Branch 16 not taken.
 1056 v_.segment(k + 1, nvt_max) -= U.row(k).segment(k + 1, nvt_max).transpose() * v_[k];
559 }
560
561 64 return v_;
562 }
563 } // namespace internal
564
565 template<
566 typename Scalar,
567 int Options,
568 template<typename, int>
569 class JointCollectionTpl,
570 typename Mat>
571 2 Mat & computeMinv(
572 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
573 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
574 const Eigen::MatrixBase<Mat> & Minv)
575 {
576
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.rows(), model.nv);
577
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv);
578
579
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
580
581 2 Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, Minv);
582
583
2/2
✓ Branch 0 taken 64 times.
✓ Branch 1 taken 2 times.
 66 for (int col = 0; col < model.nv; ++col)
584
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 internal::Miunit(model, data, col, Minv_.col(col));
585
586
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Minv_.template triangularView<Eigen::StrictlyLower>() =
587
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
588
589 2 return Minv_;
590 }
591
592 } // namespace cholesky
593 } // namespace pinocchio
594
595 /// @endcond
596
597 #endif // ifndef __pinocchio_cholesky_hxx__
598