GCC Code Coverage Report

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