GCC Code Coverage Report

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