GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-cholesky.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 276 281 98.2%
Branches: 255 568 44.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2024 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_contact_cholesky_hxx__
6 #define __pinocchio_algorithm_contact_cholesky_hxx__
7
8 #include "pinocchio/algorithm/check.hpp"
9 #include "pinocchio/multibody/data.hpp"
10
11 #include <algorithm>
12
13 namespace pinocchio
14 {
15
16 // TODO Remove when API is stabilized
17 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
18 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
19
20 template<typename Scalar, int Options>
21 template<typename S1, int O1, template<typename, int> class JointCollectionTpl, class Allocator>
22 86 void ContactCholeskyDecompositionTpl<Scalar, Options>::allocate(
23 const ModelTpl<S1, O1, JointCollectionTpl> & model,
24 const std::vector<RigidConstraintModelTpl<S1, O1>, Allocator> & contact_models)
25 {
26 typedef ModelTpl<S1, O1, JointCollectionTpl> Model;
27 typedef typename Model::JointModel JointModel;
28 typedef RigidConstraintModelTpl<S1, O1> RigidConstraintModel;
29 typedef std::vector<RigidConstraintModel, Allocator> RigidConstraintModelVector;
30
31
2/4
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
 86 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
32
33 86 nv = model.nv;
34 86 num_contacts = (Eigen::DenseIndex)contact_models.size();
35
36 86 Eigen::DenseIndex num_total_constraints = 0;
37 86 for (typename RigidConstraintModelVector::const_iterator it = contact_models.begin();
38
2/2
✓ Branch 2 taken 134 times.
✓ Branch 3 taken 86 times.
 220 it != contact_models.end(); ++it)
39 {
40
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 134 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 134 PINOCCHIO_CHECK_INPUT_ARGUMENT(
41 it->size() > 0, "The dimension of the constraint must be positive");
42 134 num_total_constraints += it->size();
43 }
44
45 86 U1inv.resize(num_total_constraints, num_total_constraints);
46 86 OSIMinv_tmp.resize(num_total_constraints, num_total_constraints);
47 86 U4inv.resize(nv, nv);
48 86 Minv_tmp.resize(nv, nv);
49
50 86 const Eigen::DenseIndex total_dim = nv + num_total_constraints;
51
52 // Compute first parents_fromRow for all the joints.
53 // This code is very similar to the code of Data::computeParents_fromRow,
54 // but shifted with a value corresponding to the number of constraints.
55 86 parents_fromRow.resize(total_dim);
56
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 parents_fromRow.fill(-1);
57
58 86 nv_subtree_fromRow.resize(total_dim);
59 // nv_subtree_fromRow.fill(0);
60
61 86 last_child.resize(model.njoints);
62
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 last_child.fill(-1);
63
2/2
✓ Branch 0 taken 2209 times.
✓ Branch 1 taken 86 times.
 2295 for (long joint_id = model.njoints - 1; joint_id >= 0; --joint_id)
64 {
65 2209 const JointIndex & parent = model.parents[(size_t)joint_id];
66
2/2
✓ Branch 1 taken 320 times.
✓ Branch 2 taken 1889 times.
 2209 if (last_child[joint_id] == -1)
67 320 last_child[joint_id] = joint_id;
68 2209 last_child[(Eigen::DenseIndex)parent] =
69 2209 std::max(last_child[joint_id], last_child[(Eigen::DenseIndex)parent]);
70 }
71
72 // Fill nv_subtree_fromRow for model
73
2/2
✓ Branch 0 taken 2123 times.
✓ Branch 1 taken 86 times.
 2209 for (JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); joint_id++)
74 {
75 2123 const JointIndex parent_id = model.parents[joint_id];
76
77 2123 const JointModel & joint = model.joints[joint_id];
78 2123 const JointModel & parent_joint = model.joints[parent_id];
79 2123 const int nvj = joint.nv();
80 2123 const int idx_vj = joint.idx_v();
81
82
2/2
✓ Branch 0 taken 2037 times.
✓ Branch 1 taken 86 times.
 2123 if (parent_id > 0)
83 2037 parents_fromRow[idx_vj + num_total_constraints] =
84 2037 parent_joint.idx_v() + parent_joint.nv() - 1 + num_total_constraints;
85 else
86 86 parents_fromRow[idx_vj + num_total_constraints] = -1;
87
88 4246 nv_subtree_fromRow[idx_vj + num_total_constraints] =
89 2123 model.joints[(size_t)last_child[(Eigen::DenseIndex)joint_id]].idx_v()
90 2123 + model.joints[(size_t)last_child[(Eigen::DenseIndex)joint_id]].nv() - idx_vj;
91
92
2/2
✓ Branch 0 taken 415 times.
✓ Branch 1 taken 2123 times.
 2538 for (int row = 1; row < nvj; ++row)
93 {
94 830 parents_fromRow[idx_vj + num_total_constraints + row] =
95 415 idx_vj + row - 1 + num_total_constraints;
96 415 nv_subtree_fromRow[idx_vj + num_total_constraints + row] =
97 415 nv_subtree_fromRow[idx_vj + num_total_constraints] - row;
98 }
99 }
100
101 86 Eigen::DenseIndex row_id = 0;
102 86 for (typename RigidConstraintModelVector::const_iterator it = contact_models.begin();
103
2/2
✓ Branch 3 taken 134 times.
✓ Branch 4 taken 86 times.
 220 it != contact_models.end(); ++it)
104 {
105 134 const RigidConstraintModel & cmodel = *it;
106 134 const JointIndex joint1_id = cmodel.joint1_id;
107 134 const JointModel & joint1 = model.joints[joint1_id];
108 134 const JointIndex joint2_id = cmodel.joint2_id;
109 134 const JointModel & joint2 = model.joints[joint2_id];
110
111 // Fill nv_subtree_fromRow for constraints
112
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 const Eigen::DenseIndex nv1 = joint1.idx_v() + joint1.nv();
113
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 const Eigen::DenseIndex nv2 = joint2.idx_v() + joint2.nv();
114 134 const Eigen::DenseIndex nv = std::max(nv1, nv2);
115
2/2
✓ Branch 1 taken 639 times.
✓ Branch 2 taken 134 times.
 773 for (Eigen::DenseIndex k = 0; k < cmodel.size(); ++k, row_id++)
116 {
117
1/2
✓ Branch 1 taken 639 times.
✗ Branch 2 not taken.
 639 nv_subtree_fromRow[row_id] = nv + (num_total_constraints - row_id);
118 }
119 }
120
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 86 times.
 86 assert(row_id == num_total_constraints);
121
122 // Fill the sparsity pattern for each Row of the Cholesky decomposition (matrix U)
123 /*
124 static const Slice default_slice_value(1,1);
125 static const SliceVector default_slice_vector(1,default_slice_value);
126
127 rowise_sparsity_pattern.clear();
128 rowise_sparsity_pattern.resize((size_t)num_total_constraints,default_slice_vector);
129 row_id = 0; size_t ee_id = 0;
130 for(typename RigidConstraintModelVector::const_iterator it = contact_models.begin();
131 it != contact_models.end();
132 ++it, ++ee_id)
133 {
134 const RigidConstraintModel & cmodel = *it;
135 const BooleanVector & joint1_indexes_ee = cmodel.colwise_joint1_sparsity;
136 const Eigen::DenseIndex contact_dim = cmodel.size();
137
138 for(Eigen::DenseIndex k = 0; k < contact_dim; ++k)
139 {
140 SliceVector & slice_vector = rowise_sparsity_pattern[(size_t)row_id];
141 slice_vector.clear();
142 slice_vector.push_back(Slice(row_id,num_total_constraints-row_id));
143
144 bool previous_index_was_true = true;
145 for(Eigen::DenseIndex joint1_indexes_ee_id = num_total_constraints;
146 joint1_indexes_ee_id < total_dim;
147 ++joint1_indexes_ee_id)
148 {
149 if(joint1_indexes_ee[joint1_indexes_ee_id])
150 {
151 if(previous_index_was_true) // no discontinuity
152 slice_vector.back().size++;
153 else // discontinuity; need to create a new slice
154 {
155 const Slice new_slice(joint1_indexes_ee_id,1);
156 slice_vector.push_back(new_slice);
157 }
158 }
159
160 previous_index_was_true = joint1_indexes_ee[joint1_indexes_ee_id];
161 }
162
163 row_id++;
164 }
165 }
166 */
167
168 // Allocate memory
169 86 D.resize(total_dim);
170 86 Dinv.resize(total_dim);
171 86 U.resize(total_dim, total_dim);
172 86 U.setIdentity();
173 86 DUt.resize(total_dim);
174 86 }
175
176 template<typename Scalar, int Options>
177 template<
178 typename S1,
179 int O1,
180 template<typename, int> class JointCollectionTpl,
181 class ConstraintModelAllocator,
182 class ConstraintDataAllocator,
183 typename VectorLike>
184 1993 void ContactCholeskyDecompositionTpl<Scalar, Options>::compute(
185 const ModelTpl<S1, O1, JointCollectionTpl> & model,
186 DataTpl<S1, O1, JointCollectionTpl> & data,
187 const std::vector<RigidConstraintModelTpl<S1, O1>, ConstraintModelAllocator> & contact_models,
188 std::vector<RigidConstraintDataTpl<S1, O1>, ConstraintDataAllocator> & contact_datas,
189 const Eigen::MatrixBase<VectorLike> & mus)
190 {
191 typedef RigidConstraintModelTpl<S1, O1> RigidConstraintModel;
192 typedef RigidConstraintDataTpl<S1, O1> RigidConstraintData;
193
194
1/2
✓ Branch 1 taken 1993 times.
✗ Branch 2 not taken.
 1993 assert(model.check(data) && "data is not consistent with model.");
195
2/4
✓ Branch 1 taken 1993 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1993 times.
✗ Branch 4 not taken.
 1993 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
196
197
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1993 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 1993 PINOCCHIO_CHECK_INPUT_ARGUMENT(
198 (Eigen::DenseIndex)contact_models.size() == num_contacts,
199 "The number of contacts inside contact_models and the one during allocation do not match.\n"
200 "Please call first ContactCholeskyDecompositionTpl::allocate method.");
201
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1993 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 1993 PINOCCHIO_CHECK_INPUT_ARGUMENT(
202 (Eigen::DenseIndex)contact_datas.size() == num_contacts,
203 "The number of contacts inside contact_datas and the one during allocation do not match.\n"
204 "Please call first ContactCholeskyDecompositionTpl::allocate method.");
205 PINOCCHIO_UNUSED_VARIABLE(model);
206
207 1993 const Eigen::DenseIndex total_dim = size();
208 1993 const Eigen::DenseIndex total_constraints_dim = total_dim - nv;
209
210 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
211 1993 const typename Data::MatrixXs & M = data.M;
212
213 1993 const size_t num_ee = contact_models.size();
214
215 // Update frame placements if needed
216
2/2
✓ Branch 0 taken 3157 times.
✓ Branch 1 taken 1993 times.
 5150 for (size_t ee_id = 0; ee_id < num_ee; ++ee_id)
217 {
218 3157 const RigidConstraintModel & cmodel = contact_models[ee_id];
219 3157 RigidConstraintData & cdata = contact_datas[ee_id];
220
221 3157 cmodel.calc(model, data, cdata);
222 }
223
224
2/4
✓ Branch 2 taken 1993 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1993 times.
✗ Branch 6 not taken.
 1993 D.tail(model.nv) = M.diagonal();
225
3/6
✓ Branch 1 taken 1993 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1993 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1993 times.
✗ Branch 8 not taken.
 1993 U.bottomRightCorner(model.nv, model.nv).template triangularView<Eigen::StrictlyUpper>() =
226 1993 M.template triangularView<Eigen::StrictlyUpper>();
227
228 // Constraint filling
229 1993 Eigen::DenseIndex current_row = 0;
230
1/2
✓ Branch 2 taken 1993 times.
✗ Branch 3 not taken.
 1993 U.topRightCorner(total_constraints_dim, model.nv).setZero();
231
2/2
✓ Branch 0 taken 3157 times.
✓ Branch 1 taken 1993 times.
 5150 for (size_t ee_id = 0; ee_id < num_ee; ++ee_id)
232 {
233 3157 const RigidConstraintModel & cmodel = contact_models[ee_id];
234 3157 RigidConstraintData & cdata = contact_datas[ee_id];
235
236 3157 const Eigen::DenseIndex constraint_dim = cmodel.size();
237
1/2
✓ Branch 1 taken 3157 times.
✗ Branch 2 not taken.
 3157 cmodel.jacobian(
238 3157 model, data, cdata, U.block(current_row, total_constraints_dim, cmodel.size(), model.nv));
239 3157 current_row += constraint_dim;
240 }
241
242 // Cholesky
243
2/2
✓ Branch 0 taken 52936 times.
✓ Branch 1 taken 1993 times.
 54929 for (Eigen::DenseIndex j = nv - 1; j >= 0; --j)
244 {
245 // Classic Cholesky decomposition related to the mass matrix
246 52936 const Eigen::DenseIndex jj = total_constraints_dim + j; // shifted index
247
1/2
✓ Branch 1 taken 52936 times.
✗ Branch 2 not taken.
 52936 const Eigen::DenseIndex NVT = nv_subtree_fromRow[jj] - 1;
248
1/2
✓ Branch 1 taken 52936 times.
✗ Branch 2 not taken.
 52936 typename Vector::SegmentReturnType DUt_partial = DUt.head(NVT);
249
250
2/2
✓ Branch 0 taken 46212 times.
✓ Branch 1 taken 6724 times.
 52936 if (NVT)
251
3/6
✓ Branch 1 taken 46212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46212 times.
✗ Branch 8 not taken.
 46212 DUt_partial.noalias() =
252
4/8
✓ Branch 1 taken 46212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 46212 times.
✗ Branch 11 not taken.
 92424 U.row(jj).segment(jj + 1, NVT).transpose().cwiseProduct(D.segment(jj + 1, NVT));
253
254
4/10
✓ Branch 1 taken 52936 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52936 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52936 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52936 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 52936 D[jj] -= U.row(jj).segment(jj + 1, NVT).dot(DUt_partial);
255
3/19
✓ Branch 1 taken 52936 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52936 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 52936 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
 52936 assert(
256 check_expression_if_real<Scalar>(D[jj] != Scalar(0))
257 && "The diagonal element is equal to zero.");
258
2/10
✓ Branch 1 taken 52936 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52936 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.
 52936 Dinv[jj] = Scalar(1) / D[jj];
259
260
3/4
✓ Branch 1 taken 52936 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 409856 times.
✓ Branch 4 taken 52936 times.
 462792 for (Eigen::DenseIndex _ii = parents_fromRow[jj]; _ii >= total_constraints_dim;
261
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 409856 _ii = parents_fromRow[_ii])
262 {
263
4/10
✓ Branch 1 taken 409856 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 409856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 409856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 409856 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 409856 U(_ii, jj) -= U.row(_ii).segment(jj + 1, NVT).dot(DUt_partial);
264
3/6
✓ Branch 1 taken 409856 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 409856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 409856 times.
✗ Branch 8 not taken.
 409856 U(_ii, jj) *= Dinv[jj];
265 }
266
267 // Constraint part
268 // Eigen::DenseIndex current_row = total_constraints_dim - 1;
269 // for(size_t ee_id = 0; ee_id < num_ee; ++ee_id)
270 // {
271 // const RigidConstraintModel & cmodel = contact_models[num_ee - 1 - ee_id];
272 //
273 // const Eigen::DenseIndex constraint_dim = cmodel.size();
274 // if(cmodel.colwise_sparsity[j])
275 // {
276 // for(Eigen::DenseIndex k = 0; k < constraint_dim; ++k)
277 // {
278 // U(current_row - k,jj) -= U.row(current_row -
279 // k).segment(jj+1,NVT).dot(DUt_partial); U(current_row - k,jj) *= Dinv[jj];
280 // }
281 // }
282 //
283 // current_row -= constraint_dim;
284 // }
285
2/2
✓ Branch 0 taken 322992 times.
✓ Branch 1 taken 52936 times.
 375928 for (Eigen::DenseIndex current_row = total_constraints_dim - 1; current_row >= 0;
286 --current_row)
287 {
288
4/10
✓ Branch 1 taken 322992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 322992 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 322992 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 322992 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 322992 U(current_row, jj) -= U.row(current_row).segment(jj + 1, NVT).dot(DUt_partial);
289
2/6
✓ Branch 1 taken 322992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 322992 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 322992 U(current_row, jj) *= Dinv[jj];
290 }
291 }
292
293 1993 updateDamping(mus);
294 1993 }
295
296 template<typename Scalar, int Options>
297 template<typename VectorLike>
298 1995 void ContactCholeskyDecompositionTpl<Scalar, Options>::updateDamping(
299 const Eigen::MatrixBase<VectorLike> & vec)
300 {
301 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike)
302 1995 const Eigen::DenseIndex total_dim = size();
303 1995 const Eigen::DenseIndex total_constraints_dim = total_dim - nv;
304
305 // Upper left triangular part of U
306
2/2
✓ Branch 0 taken 13113 times.
✓ Branch 1 taken 1995 times.
 15108 for (Eigen::DenseIndex j = total_constraints_dim - 1; j >= 0; --j)
307 {
308 13113 const Eigen::DenseIndex slice_dim = total_dim - j - 1;
309
1/2
✓ Branch 1 taken 13113 times.
✗ Branch 2 not taken.
 13113 typename Vector::SegmentReturnType DUt_partial = DUt.head(slice_dim);
310
3/6
✓ Branch 1 taken 13113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13113 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13113 times.
✗ Branch 8 not taken.
 13113 DUt_partial.noalias() =
311
4/8
✓ Branch 1 taken 13113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13113 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13113 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13113 times.
✗ Branch 11 not taken.
 13113 U.row(j).segment(j + 1, slice_dim).transpose().cwiseProduct(D.segment(j + 1, slice_dim));
312
313
5/16
✓ Branch 1 taken 13113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13113 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13113 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13113 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13113 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 13113 D[j] = -vec[j] - U.row(j).segment(j + 1, slice_dim).dot(DUt_partial);
314
3/19
✓ Branch 1 taken 13113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13113 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13113 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
 13113 assert(
315 check_expression_if_real<Scalar>(D[j] != Scalar(0))
316 && "The diagonal element is equal to zero.");
317
2/10
✓ Branch 1 taken 13113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13113 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.
 13113 Dinv[j] = Scalar(1) / D[j];
318
319
2/2
✓ Branch 0 taken 51957 times.
✓ Branch 1 taken 13113 times.
 65070 for (Eigen::DenseIndex _i = j - 1; _i >= 0; _i--)
320 {
321
5/16
✓ Branch 1 taken 51957 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51957 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51957 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51957 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 51957 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
 51957 U(_i, j) = -U.row(_i).segment(j + 1, slice_dim).dot(DUt_partial) * Dinv[j];
322 }
323 }
324 1995 }
325
326 template<typename Scalar, int Options>
327 2 void ContactCholeskyDecompositionTpl<Scalar, Options>::updateDamping(const Scalar & mu)
328 {
329 // PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real<Scalar>(mu >= 0), "mu should be
330 // positive.");
331
332 2 const Eigen::DenseIndex total_dim = size();
333 2 const Eigen::DenseIndex total_constraints_dim = total_dim - nv;
334
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 updateDamping(Vector::Constant(total_constraints_dim, mu));
335 2 }
336
337 template<typename Scalar, int Options>
338 template<typename MatrixLike>
339 3065 void ContactCholeskyDecompositionTpl<Scalar, Options>::solveInPlace(
340 const Eigen::MatrixBase<MatrixLike> & mat) const
341 {
342 3065 MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike, mat);
343
344 3065 Uiv(mat_);
345
3/6
✓ Branch 2 taken 3065 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3065 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3065 times.
✗ Branch 9 not taken.
 3065 mat_.array().colwise() *= Dinv.array();
346 3065 Utiv(mat_);
347 3065 }
348
349 template<typename Scalar, int Options>
350 template<typename MatrixLike>
351 typename ContactCholeskyDecompositionTpl<Scalar, Options>::Matrix
352 10 ContactCholeskyDecompositionTpl<Scalar, Options>::solve(
353 const Eigen::MatrixBase<MatrixLike> & mat) const
354 {
355 10 Matrix res(mat);
356
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 solveInPlace(res);
357 10 return res;
358 }
359
360 template<typename Scalar, int Options>
361 template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
362 ContactCholeskyDecompositionTpl<Scalar, Options>
363 1 ContactCholeskyDecompositionTpl<Scalar, Options>::getMassMatrixChoeslkyDecomposition(
364 const ModelTpl<S1, O1, JointCollectionTpl> & model) const
365 {
366 typedef ContactCholeskyDecompositionTpl<Scalar, Options> ReturnType;
367 1 ReturnType res(model);
368
369
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res.D = D.tail(nv);
370
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res.Dinv = Dinv.tail(nv);
371
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res.U = U.bottomRightCorner(nv, nv);
372
373 1 return res;
374 }
375
376 namespace details
377 {
378 template<typename MatrixLike, int ColsAtCompileTime>
379 struct UvAlgo
380 {
381 template<typename Scalar, int Options>
382 4 static void run(
383 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
384 const Eigen::MatrixBase<MatrixLike> & mat)
385 {
386 4 MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike, mat);
387
388
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
389 mat.rows() == chol.size(), "The input matrix is of wrong size");
390
391
2/2
✓ Branch 1 taken 80 times.
✓ Branch 2 taken 4 times.
 84 for (Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id)
392
1/2
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
 80 UvAlgo<typename MatrixLike::ColXpr>::run(chol, mat_.col(col_id));
393 4 }
394 };
395
396 template<typename VectorLike>
397 struct UvAlgo<VectorLike, 1>
398 {
399 template<typename Scalar, int Options>
400 84 static void run(
401 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
402 const Eigen::MatrixBase<VectorLike> & vec)
403 {
404 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike)
405 84 VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike, vec);
406
407
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 84 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 84 PINOCCHIO_CHECK_INPUT_ARGUMENT(
408 vec.size() == chol.size(), "The input vector is of wrong size");
409 84 const Eigen::DenseIndex num_total_constraints = chol.size() - chol.nv;
410
411 // TODO: exploit the Sparsity pattern of the first rows of U
412
2/2
✓ Branch 0 taken 945 times.
✓ Branch 1 taken 84 times.
 1029 for (Eigen::DenseIndex k = 0; k < num_total_constraints; ++k)
413 {
414 945 const Eigen::DenseIndex slice_dim = chol.size() - k - 1;
415
4/10
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 945 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 945 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 945 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 945 vec_[k] += chol.U.row(k).tail(slice_dim).dot(vec_.tail(slice_dim));
416 }
417
418
2/2
✓ Branch 1 taken 2604 times.
✓ Branch 2 taken 84 times.
 2688 for (Eigen::DenseIndex k = num_total_constraints; k <= chol.size() - 2; ++k)
419
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.
 2604 vec_[k] += chol.U.row(k)
420
2/4
✓ Branch 1 taken 2604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2604 times.
✗ Branch 5 not taken.
 2604 .segment(k + 1, chol.nv_subtree_fromRow[k] - 1)
421
4/8
✓ Branch 1 taken 2604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2604 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2604 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2604 times.
✗ Branch 11 not taken.
 2604 .dot(vec_.segment(k + 1, chol.nv_subtree_fromRow[k] - 1));
422 84 }
423 };
424 } // namespace details
425
426 template<typename Scalar, int Options>
427 template<typename MatrixLike>
428 8 void ContactCholeskyDecompositionTpl<Scalar, Options>::Uv(
429 const Eigen::MatrixBase<MatrixLike> & mat) const
430 {
431 8 details::UvAlgo<MatrixLike>::run(*this, mat.const_cast_derived());
432 8 }
433
434 namespace details
435 {
436 template<typename MatrixLike, int ColsAtCompileTime>
437 struct UtvAlgo
438 {
439 template<typename Scalar, int Options>
440 4 static void run(
441 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
442 const Eigen::MatrixBase<MatrixLike> & mat)
443 {
444 4 MatrixLike & mat_ = mat.const_cast_derived();
445
446
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
447 mat.rows() == chol.size(), "The input matrix is of wrong size");
448
449
2/2
✓ Branch 1 taken 80 times.
✓ Branch 2 taken 4 times.
 84 for (Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id)
450
1/2
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
 80 UtvAlgo<typename MatrixLike::ColXpr>::run(chol, mat_.col(col_id));
451 4 }
452 };
453
454 template<typename VectorLike>
455 struct UtvAlgo<VectorLike, 1>
456 {
457 template<typename Scalar, int Options>
458 84 static void run(
459 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
460 const Eigen::MatrixBase<VectorLike> & vec)
461 {
462 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike)
463 84 VectorLike & vec_ = vec.const_cast_derived();
464
465
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 84 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 84 PINOCCHIO_CHECK_INPUT_ARGUMENT(
466 vec.size() == chol.size(), "The input vector is of wrong size");
467 84 const Eigen::DenseIndex num_total_constraints = chol.constraintDim();
468
469
2/2
✓ Branch 1 taken 2604 times.
✓ Branch 2 taken 84 times.
 2688 for (Eigen::DenseIndex k = chol.size() - 2; k >= num_total_constraints; --k)
470
4/8
✓ Branch 1 taken 2604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2604 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2604 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2604 times.
✗ Branch 11 not taken.
 2604 vec_.segment(k + 1, chol.nv_subtree_fromRow[k] - 1) +=
471
3/6
✓ Branch 3 taken 2604 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2604 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2604 times.
✗ Branch 10 not taken.
 5208 chol.U.row(k).segment(k + 1, chol.nv_subtree_fromRow[k] - 1).transpose() * vec_[k];
472
473 // TODO: exploit the Sparsity pattern of the first rows of U
474
2/2
✓ Branch 0 taken 945 times.
✓ Branch 1 taken 84 times.
 1029 for (Eigen::DenseIndex k = num_total_constraints - 1; k >= 0; --k)
475 {
476 945 const Eigen::DenseIndex slice_dim = chol.size() - k - 1;
477
5/10
✓ Branch 3 taken 945 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 945 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 945 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 945 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 945 times.
✗ Branch 16 not taken.
 945 vec_.tail(slice_dim) += chol.U.row(k).tail(slice_dim).transpose() * vec_[k];
478 }
479 84 }
480 };
481 } // namespace details
482
483 template<typename Scalar, int Options>
484 template<typename MatrixLike>
485 8 void ContactCholeskyDecompositionTpl<Scalar, Options>::Utv(
486 const Eigen::MatrixBase<MatrixLike> & mat) const
487 {
488 8 details::UtvAlgo<MatrixLike>::run(*this, mat.const_cast_derived());
489 8 }
490
491 namespace details
492 {
493 template<typename MatrixLike, int ColsAtCompileTime>
494 struct UivAlgo
495 {
496 template<typename Scalar, int Options>
497 18 static void run(
498 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
499 const Eigen::MatrixBase<MatrixLike> & mat)
500 {
501 18 MatrixLike & mat_ = mat.const_cast_derived();
502
503
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 18 PINOCCHIO_CHECK_INPUT_ARGUMENT(
504 mat.rows() == chol.size(), "The input matrix is of wrong size");
505
506
2/2
✓ Branch 1 taken 263 times.
✓ Branch 2 taken 18 times.
 281 for (Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id)
507
1/2
✓ Branch 2 taken 263 times.
✗ Branch 3 not taken.
 263 UivAlgo<typename MatrixLike::ColXpr>::run(chol, mat_.col(col_id));
508 18 }
509 };
510
511 template<typename VectorLike>
512 struct UivAlgo<VectorLike, 1>
513 {
514 template<typename Scalar, int Options>
515 3319 static void run(
516 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
517 const Eigen::MatrixBase<VectorLike> & vec)
518 {
519 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike)
520 3319 VectorLike & vec_ = vec.const_cast_derived();
521
522
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 3319 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 3319 PINOCCHIO_CHECK_INPUT_ARGUMENT(
523 vec.size() == chol.size(), "The input vector is of wrong size");
524
525 3319 const Eigen::DenseIndex num_total_constraints = chol.size() - chol.nv;
526
2/2
✓ Branch 1 taken 92104 times.
✓ Branch 2 taken 3319 times.
 95423 for (Eigen::DenseIndex k = chol.size() - 2; k >= num_total_constraints; --k)
527
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.
 92104 vec_[k] -= chol.U.row(k)
528
2/4
✓ Branch 1 taken 92104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92104 times.
✗ Branch 5 not taken.
 92104 .segment(k + 1, chol.nv_subtree_fromRow[k] - 1)
529
4/8
✓ Branch 1 taken 92104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92104 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 92104 times.
✗ Branch 11 not taken.
 92104 .dot(vec_.segment(k + 1, chol.nv_subtree_fromRow[k] - 1));
530
531 // TODO: exploit the Sparsity pattern of the first rows of U
532
2/2
✓ Branch 0 taken 19944 times.
✓ Branch 1 taken 3319 times.
 23263 for (Eigen::DenseIndex k = num_total_constraints - 1; k >= 0; --k)
533 {
534 19944 const Eigen::DenseIndex slice_dim = chol.size() - k - 1;
535
4/10
✓ Branch 2 taken 19944 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 19944 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 19944 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19944 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 19944 vec_[k] -= chol.U.row(k).tail(slice_dim).dot(vec_.tail(slice_dim));
536 }
537 3319 }
538 };
539 } // namespace details
540
541 template<typename Scalar, int Options>
542 template<typename MatrixLike>
543 3074 void ContactCholeskyDecompositionTpl<Scalar, Options>::Uiv(
544 const Eigen::MatrixBase<MatrixLike> & mat) const
545 {
546 3074 details::UivAlgo<MatrixLike>::run(*this, mat.const_cast_derived());
547 3074 }
548
549 namespace details
550 {
551 template<typename MatrixLike, int ColsAtCompileTime>
552 struct UtivAlgo
553 {
554 template<typename Scalar, int Options>
555 18 static void run(
556 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
557 const Eigen::MatrixBase<MatrixLike> & mat)
558 {
559 18 MatrixLike & mat_ = mat.const_cast_derived();
560
561
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 18 PINOCCHIO_CHECK_INPUT_ARGUMENT(
562 mat.rows() == chol.size(), "The input matrix is of wrong size");
563
564
2/2
✓ Branch 1 taken 263 times.
✓ Branch 2 taken 18 times.
 281 for (Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id)
565
1/2
✓ Branch 2 taken 263 times.
✗ Branch 3 not taken.
 263 UtivAlgo<typename MatrixLike::ColXpr>::run(chol, mat_.col(col_id));
566 18 }
567 };
568
569 template<typename VectorLike>
570 struct UtivAlgo<VectorLike, 1>
571 {
572 template<typename Scalar, int Options>
573 3319 static void run(
574 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
575 const Eigen::MatrixBase<VectorLike> & vec)
576 {
577 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike)
578 3319 VectorLike & vec_ = vec.const_cast_derived();
579
580
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 3319 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 3319 PINOCCHIO_CHECK_INPUT_ARGUMENT(
581 vec.size() == chol.size(), "The input vector is of wrong size");
582 3319 const Eigen::DenseIndex num_total_constraints = chol.constraintDim();
583
584 // TODO: exploit the Sparsity pattern of the first rows of U
585
2/2
✓ Branch 0 taken 19944 times.
✓ Branch 1 taken 3319 times.
 23263 for (Eigen::DenseIndex k = 0; k < num_total_constraints; ++k)
586 {
587 19944 const Eigen::DenseIndex slice_dim = chol.size() - k - 1;
588
5/10
✓ Branch 3 taken 19944 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19944 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19944 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 19944 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 19944 times.
✗ Branch 16 not taken.
 19944 vec_.tail(slice_dim) -= chol.U.row(k).tail(slice_dim).transpose() * vec_[k];
589 }
590
591
2/2
✓ Branch 1 taken 92104 times.
✓ Branch 2 taken 3319 times.
 95423 for (Eigen::DenseIndex k = num_total_constraints; k <= chol.size() - 2; ++k)
592
4/8
✓ Branch 1 taken 92104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 92104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 92104 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 92104 times.
✗ Branch 11 not taken.
 92104 vec_.segment(k + 1, chol.nv_subtree_fromRow[k] - 1) -=
593
3/6
✓ Branch 3 taken 92104 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 92104 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 92104 times.
✗ Branch 10 not taken.
 184208 chol.U.row(k).segment(k + 1, chol.nv_subtree_fromRow[k] - 1).transpose() * vec_[k];
594 3319 }
595 };
596 } // namespace details
597
598 template<typename Scalar, int Options>
599 template<typename MatrixLike>
600 3074 void ContactCholeskyDecompositionTpl<Scalar, Options>::Utiv(
601 const Eigen::MatrixBase<MatrixLike> & mat) const
602 {
603 3074 details::UtivAlgo<MatrixLike>::run(*this, PINOCCHIO_EIGEN_CONST_CAST(MatrixLike, mat));
604 3074 }
605
606 template<typename Scalar, int Options>
607 typename ContactCholeskyDecompositionTpl<Scalar, Options>::Matrix
608 53 ContactCholeskyDecompositionTpl<Scalar, Options>::matrix() const
609 {
610
2/5
✓ Branch 2 taken 53 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 53 times.
✗ Branch 6 not taken.
 53 Matrix res(size(), size());
611
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 matrix(res);
612 53 return res;
613 }
614
615 template<typename Scalar, int Options>
616 template<typename MatrixType>
617 57 void ContactCholeskyDecompositionTpl<Scalar, Options>::matrix(
618 const Eigen::MatrixBase<MatrixType> & res) const
619 {
620 57 MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res);
621
5/10
✓ Branch 2 taken 57 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 57 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 57 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 57 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 57 times.
✗ Branch 15 not taken.
 57 res_.noalias() = U * D.asDiagonal() * U.transpose();
622 57 }
623
624 template<typename Scalar, int Options>
625 typename ContactCholeskyDecompositionTpl<Scalar, Options>::Matrix
626 ✗ ContactCholeskyDecompositionTpl<Scalar, Options>::inverse() const
627 {
628 ✗ Matrix res(size(), size());
629 ✗ inverse(res);
630 ✗ return res;
631 }
632
633 template<typename Scalar, int Options>
634 template<typename S1, int O1>
635 3 bool ContactCholeskyDecompositionTpl<Scalar, Options>::operator==(
636 const ContactCholeskyDecompositionTpl<S1, O1> & other) const
637 {
638 3 bool is_same = true;
639
640
3/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 3 if (nv != other.nv || num_contacts != other.num_contacts)
641 1 return false;
642
643 2 if (
644
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 4 D.size() != other.D.size() || Dinv.size() != other.Dinv.size() || U.rows() != other.U.rows()
645
3/6
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 4 || U.cols() != other.U.cols())
646 ✗ return false;
647
648 2 is_same &= (D == other.D);
649 2 is_same &= (Dinv == other.Dinv);
650 2 is_same &= (U == other.U);
651
652 2 is_same &= (parents_fromRow == other.parents_fromRow);
653 2 is_same &= (nv_subtree_fromRow == other.nv_subtree_fromRow);
654 2 is_same &= (last_child == other.last_child);
655 // is_same &= (rowise_sparsity_pattern == other.rowise_sparsity_pattern);
656
657 2 return is_same;
658 }
659
660 template<typename Scalar, int Options>
661 template<typename S1, int O1>
662 1 bool ContactCholeskyDecompositionTpl<Scalar, Options>::operator!=(
663 const ContactCholeskyDecompositionTpl<S1, O1> & other) const
664 {
665 1 return !(*this == other);
666 }
667
668 namespace details
669 {
670
671 template<typename Scalar, int Options, typename VectorLike>
672 598 PINOCCHIO_DONT_INLINE VectorLike & inverseAlgo(
673 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
674 const Eigen::DenseIndex col,
675 const Eigen::MatrixBase<VectorLike> & vec)
676 {
677 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
678
679 typedef ContactCholeskyDecompositionTpl<Scalar, Options> ContactCholeskyDecomposition;
680 typedef typename ContactCholeskyDecomposition::RowMatrix RowMatrix;
681
682
1/2
✓ Branch 1 taken 299 times.
✗ Branch 2 not taken.
 598 const Eigen::DenseIndex & chol_dim = chol.size();
683
2/6
✓ Branch 0 taken 299 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 299 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 598 PINOCCHIO_CHECK_INPUT_ARGUMENT(col < chol_dim && col >= 0);
684
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 299 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 598 PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == chol_dim);
685
686 598 const typename ContactCholeskyDecomposition::IndexVector & nvt = chol.nv_subtree_fromRow;
687 598 VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike, vec);
688
689 598 const Eigen::DenseIndex last_col =
690 598 std::min(col - 1, chol_dim - 2); // You can start from nv-2 (no child in nv-1)
691
1/6
✓ Branch 1 taken 299 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 598 vec_[col] = Scalar(1);
692
2/4
✓ Branch 1 taken 299 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 299 times.
✗ Branch 5 not taken.
 598 vec_.tail(chol_dim - col - 1).setZero();
693
694 // TODO: exploit the sparsity pattern of the first rows of U
695
2/2
✓ Branch 0 taken 6397 times.
✓ Branch 1 taken 299 times.
 13392 for (Eigen::DenseIndex k = last_col; k >= 0; --k)
696 {
697
1/2
✓ Branch 1 taken 6397 times.
✗ Branch 2 not taken.
 12794 const Eigen::DenseIndex nvt_max = std::min(col - k, nvt[k] - 1);
698
1/2
✓ Branch 1 taken 6397 times.
✗ Branch 2 not taken.
 12794 const typename RowMatrix::ConstRowXpr U_row = chol.U.row(k);
699
4/12
✓ Branch 1 taken 6397 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6397 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6397 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6397 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 12794 vec_[k] = -U_row.segment(k + 1, nvt_max).dot(vec_.segment(k + 1, nvt_max));
700 // if(k >= chol_constraint_dim)
701 // {
702 // vec_[k] = -U_row.segment(k+1,nvt_max).dot(vec_.segment(k+1,nvt_max));
703 // }
704 // else
705 // {
706 // typedef typename ContactCholeskyDecomposition::SliceVector SliceVector;
707 // typedef typename ContactCholeskyDecomposition::Slice Slice;
708 // const SliceVector & slice_vector = chol.rowise_sparsity_pattern[(size_t)k];
709 //
710 // const Slice & slice_0 = slice_vector[0];
711 // assert(slice_0.first_index == k);
712 // Eigen::DenseIndex last_index1 = slice_0.first_index + slice_0.size;
713 // const Eigen::DenseIndex last_index2 = k + nvt_max;
714 // Eigen::DenseIndex slice_dim = std::min(last_index1,last_index2) - k;
715 // vec_[k] =
716 // -U_row.segment(slice_0.first_index+1,slice_dim-1).dot(vec_.segment(slice_0.first_index+1,slice_dim-1));
717 //
718 // typename SliceVector::const_iterator slice_it = slice_vector.begin()++;
719 // for(;slice_it != slice_vector.end(); ++slice_it)
720 // {
721 // const Slice & slice = *slice_it;
722 // last_index1 = slice.first_index + slice.size;
723 // slice_dim = std::min(last_index1,last_index2+1) - slice.first_index;
724 // if(slice_dim <= 0) break;
725 //
726 // vec_[k] -=
727 // U_row.segment(slice.first_index,slice_dim).dot(vec_.segment(slice.first_index,slice_dim));
728 // }
729 // }
730 }
731
732
5/10
✓ Branch 1 taken 299 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 299 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 299 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 299 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 299 times.
✗ Branch 14 not taken.
 598 vec_.head(col + 1).array() *= chol.Dinv.head(col + 1).array();
733
734
2/2
✓ Branch 0 taken 6696 times.
✓ Branch 1 taken 299 times.
 13990 for (Eigen::DenseIndex k = 0; k < col + 1; ++k) // You can stop one step before nv.
735 {
736
1/2
✓ Branch 1 taken 6696 times.
✗ Branch 2 not taken.
 13392 const Eigen::DenseIndex nvt_max = nvt[k] - 1;
737
7/14
✓ Branch 1 taken 6696 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6696 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6696 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6696 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6696 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6696 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6696 times.
✗ Branch 20 not taken.
 13392 vec_.segment(k + 1, nvt_max) -= chol.U.row(k).segment(k + 1, nvt_max).transpose() * vec_[k];
738 }
739
740 598 return vec_;
741 }
742 } // namespace details
743
744 template<typename Scalar, int Options>
745 template<typename MatrixType>
746 7 void ContactCholeskyDecompositionTpl<Scalar, Options>::inverse(
747 const Eigen::MatrixBase<MatrixType> & res) const
748 {
749
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 7 PINOCCHIO_CHECK_INPUT_ARGUMENT(res.rows() == size());
750
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 7 PINOCCHIO_CHECK_INPUT_ARGUMENT(res.cols() == size());
751
752 7 MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res);
753
754
2/2
✓ Branch 1 taken 299 times.
✓ Branch 2 taken 7 times.
 306 for (Eigen::DenseIndex col_id = 0; col_id < size(); ++col_id)
755
1/2
✓ Branch 2 taken 299 times.
✗ Branch 3 not taken.
 299 details::inverseAlgo(*this, col_id, res_.col(col_id));
756
757
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 res_.template triangularView<Eigen::StrictlyLower>() =
758
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 res_.transpose().template triangularView<Eigen::StrictlyLower>();
759 7 }
760
761 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
762 } // namespace pinocchio
763
764 #endif // ifndef __pinocchio_algorithm_contact_cholesky_hxx__
765