GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-cholesky.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 98 123 79.7%
Branches: 149 414 36.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2024 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_contact_cholesky_hpp__
6 #define __pinocchio_algorithm_contact_cholesky_hpp__
7
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/math/matrix-block.hpp"
10 #include "pinocchio/math/triangular-matrix.hpp"
11
12 #include "pinocchio/algorithm/contact-info.hpp"
13 #include "pinocchio/algorithm/delassus-operator-base.hpp"
14
15 namespace pinocchio
16 {
17
18 // Forward declaration of algo
19 namespace details
20 {
21 template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
22 struct UvAlgo;
23
24 template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
25 struct UtvAlgo;
26
27 template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
28 struct UivAlgo;
29
30 template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
31 struct UtivAlgo;
32
33 template<typename Scalar, int Options, typename VectorLike>
34 VectorLike & inverseAlgo(
35 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
36 const Eigen::DenseIndex col,
37 const Eigen::MatrixBase<VectorLike> & vec);
38 } // namespace details
39
40 template<typename _ContactCholeskyDecomposition>
41 struct DelassusCholeskyExpressionTpl;
42
43 ///
44 ///  \brief Contact Cholesky decomposition structure. This structure allows
45 /// to compute in a efficient and parsimonious way the Cholesky decomposition
46 /// of the KKT matrix related to the contact dynamics.
47 /// Such a decomposition is usefull when computing both the forward dynamics in contact
48 /// or the related analytical derivatives.
49 ///
50 ///
51 /// \tparam _Scalar Scalar type.
52 ///  \tparam _Options Alignment Options of the Eigen objects contained in the data structure.
53 ///
54 template<typename _Scalar, int _Options>
55 struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
56 ContactCholeskyDecompositionTpl
57 {
58 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59
60 typedef pinocchio::Index Index;
61 typedef _Scalar Scalar;
62 enum
63 {
64 LINEAR = 0,
65 ANGULAR = 3,
66 Options = _Options
67 };
68
69 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> Vector;
70 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> Matrix;
71 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix) RowMatrix;
72 // TODO Remove when API is stabilized
73 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
74 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
75 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
76 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
77 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
78 typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 1, Options> IndexVector;
79 typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(IndexVector) VectorOfIndexVector;
80 typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Options> BooleanVector;
81
82 ///@{
83 /// \brief Data information related to the Sparsity structure of the Cholesky decompostion
84 struct Slice
85 {
86 ✗ Slice(const Eigen::DenseIndex & first_index, const Eigen::DenseIndex & size)
87 ✗ : first_index(first_index)
88 ✗ , size(size)
89 {
90 }
91
92 Eigen::DenseIndex first_index;
93 Eigen::DenseIndex size;
94 };
95
96 typedef DelassusCholeskyExpressionTpl<ContactCholeskyDecompositionTpl>
97 DelassusCholeskyExpression;
98 friend struct DelassusCholeskyExpressionTpl<ContactCholeskyDecompositionTpl>;
99
100 typedef std::vector<Slice> SliceVector;
101 typedef std::vector<SliceVector> VectorOfSliceVector;
102 ///@}
103
104 ///
105 /// \brief Default constructor
106 ///
107
10/20
✓ Branch 2 taken 882 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 882 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 882 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 882 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 882 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 882 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 882 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 882 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 882 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 882 times.
✗ Branch 31 not taken.
 882 ContactCholeskyDecompositionTpl() = default;
108
109 ///
110 /// \brief Constructor from a model.
111 ///
112 /// \param[in] model Model of the kinematic tree.
113 ///
114 template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
115 3 explicit ContactCholeskyDecompositionTpl(const ModelTpl<S1, O1, JointCollectionTpl> & model)
116
10/20
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
 3 {
117 // TODO Remove when API is stabilized
118 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
119 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
120
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models;
121 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
122
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 allocate(model, empty_contact_models);
123 3 }
124
125 ///
126 /// \brief Constructor from a model and a collection of RigidConstraintModel objects.
127 ///
128 /// \param[in] model Model of the kinematic tree
129 /// \param[in] contact_models Vector of RigidConstraintModel objects containing the contact
130 /// information
131 ///
132 // TODO Remove when API is stabilized
133 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
134 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
135 template<typename S1, int O1, template<typename, int> class JointCollectionTpl, class Allocator>
136 ✗ ContactCholeskyDecompositionTpl(
137 const ModelTpl<S1, O1, JointCollectionTpl> & model,
138 const std::vector<RigidConstraintModelTpl<S1, O1>, Allocator> & contact_models)
139 {
140 ✗ allocate(model, contact_models);
141 }
142 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
143
144 ///
145 /// \brief Copy constructor
146
11/22
✓ Branch 2 taken 217 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 217 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 217 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 217 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 217 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 217 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 217 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 217 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 217 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 217 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 217 times.
✗ Branch 33 not taken.
 217 ContactCholeskyDecompositionTpl(const ContactCholeskyDecompositionTpl & copy) = default;
147
148 ///
149 ///  \brief Memory allocation of the vectors D, Dinv, and the upper triangular matrix U.
150 ///
151 /// \param[in] model Model of the kinematic tree
152 /// \param[in] contact_models Vector of RigidConstraintModel objects containing the contact
153 /// information
154 ///
155 // TODO Remove when API is stabilized
156 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
157 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
158 template<typename S1, int O1, template<typename, int> class JointCollectionTpl, class Allocator>
159 void allocate(
160 const ModelTpl<S1, O1, JointCollectionTpl> & model,
161 const std::vector<RigidConstraintModelTpl<S1, O1>, Allocator> & contact_models);
162 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
163
164 ///
165 /// \brief Returns the Inverse of the Operational Space Inertia Matrix resulting from the
166 /// decomposition.
167 ///
168 9 Matrix getInverseOperationalSpaceInertiaMatrix() const
169 {
170
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 9 Matrix res(constraintDim(), constraintDim());
171
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 getInverseOperationalSpaceInertiaMatrix(res);
172 9 return res;
173 }
174
175 template<typename MatrixType>
176 9 void getInverseOperationalSpaceInertiaMatrix(const Eigen::MatrixBase<MatrixType> & res) const
177 {
178 typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
179 ConstBlockXpr;
180 // typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr;
181
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 const ConstBlockXpr U1 = U.topLeftCorner(constraintDim(), constraintDim());
182
183 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
184 9 MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res);
185
7/14
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
 9 OSIMinv_tmp.noalias() = D.head(constraintDim()).asDiagonal() * U1.adjoint();
186
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 9 res_.noalias() = -U1 * OSIMinv_tmp;
187 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
188 9 }
189
190 /// \brief Returns the Cholesky decomposition expression associated to the underlying Delassus
191 /// matrix.
192 2915 DelassusCholeskyExpression getDelassusCholeskyExpression() const
193 {
194 2915 return DelassusCholeskyExpression(*this);
195 }
196
197 ///
198 /// \brief Returns the Operational Space Inertia Matrix resulting from the decomposition.
199 ///
200 9 Matrix getOperationalSpaceInertiaMatrix() const
201 {
202
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 9 Matrix res(constraintDim(), constraintDim());
203
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 getOperationalSpaceInertiaMatrix(res);
204 9 return res;
205 }
206
207 template<typename MatrixType>
208 35 void getOperationalSpaceInertiaMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
209 {
210 35 MatrixType & res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res_);
211 typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
212 ConstBlockXpr;
213 // typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr;
214 const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U1 =
215
3/6
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
 35 U.topLeftCorner(constraintDim(), constraintDim())
216
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 .template triangularView<Eigen::UnitUpper>();
217
218 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
219
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 U1inv.setIdentity();
220
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 U1.solveInPlace(U1inv); // TODO: implement Sparse Inverse
221
8/16
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 35 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 35 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 35 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 35 times.
✗ Branch 23 not taken.
 35 OSIMinv_tmp.noalias() = -U1inv.adjoint() * Dinv.head(constraintDim()).asDiagonal();
222
3/6
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
 35 res.noalias() = OSIMinv_tmp * U1inv;
223 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
224 35 }
225
226 ✗ Matrix getInverseMassMatrix() const
227 {
228 ✗ Matrix res(nv, nv);
229 ✗ getInverseMassMatrix(res);
230 ✗ return res;
231 }
232
233 template<typename MatrixType>
234 34 void getInverseMassMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
235 {
236 34 MatrixType & res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res_);
237 typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
238 ConstBlockXpr;
239 // typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr;
240 const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U4 =
241
2/4
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
 34 U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
242
243 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
244
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 U4inv.setIdentity();
245
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
246
6/12
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 34 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 34 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 34 times.
✗ Branch 17 not taken.
 34 Minv_tmp.noalias() = U4inv.adjoint() * Dinv.tail(nv).asDiagonal();
247
3/6
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34 times.
✗ Branch 8 not taken.
 34 res.noalias() = Minv_tmp * U4inv;
248 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
249 34 }
250
251 template<typename MatrixType>
252 7 void getJMinv(const Eigen::MatrixBase<MatrixType> & res_) const
253 {
254 7 MatrixType & res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res_);
255 typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
256 ConstBlockXpr;
257 const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U4 =
258
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
259
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 ConstBlockXpr U2 = U.topRightCorner(constraintDim(), nv);
260 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
261
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 U4inv.setIdentity();
262
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
263
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 res.noalias() = U2 * U4inv;
264 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
265 7 }
266
267 ///
268 /// \brief Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix
269 /// related to the system mass matrix and the Jacobians of the contact patches contained
270 /// in the vector of RigidConstraintModel named contact_models.
271 ///
272 /// \param[in] model Model of the dynamical system
273 /// \param[in] data Data related to model containing the computed mass matrix and the Jacobian
274 /// of the kinematic tree \param[in] contact_models Vector containing the contact models (which
275 /// frame is in contact and the type of contact: ponctual, 6D rigid, etc.) \param[out]
276 /// contact_datas Vector containing the contact data related to the contact_models. \param[in]
277 /// mu Positive regularization factor allowing to enforce the definite property of the KKT
278 /// matrix.
279 ///
280 /// \remarks The mass matrix and the Jacobians of the dynamical system should have been computed
281 /// first. This can be achieved by simply calling pinocchio::crba.
282 ///
283 // TODO Remove when API is stabilized
284 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
285 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
286 template<
287 typename S1,
288 int O1,
289 template<typename, int> class JointCollectionTpl,
290 class ConstraintModelAllocator,
291 class ConstraintDataAllocator>
292 1985 void compute(
293 const ModelTpl<S1, O1, JointCollectionTpl> & model,
294 DataTpl<S1, O1, JointCollectionTpl> & data,
295 const std::vector<RigidConstraintModelTpl<S1, O1>, ConstraintModelAllocator> & contact_models,
296 std::vector<RigidConstraintDataTpl<S1, O1>, ConstraintDataAllocator> & contact_datas,
297 const S1 mu = S1(0.))
298 {
299
1/2
✓ Branch 3 taken 1985 times.
✗ Branch 4 not taken.
 1985 compute(model, data, contact_models, contact_datas, Vector::Constant(U1inv.rows(), mu));
300 1985 }
301
302 ///
303 /// \brief Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix
304 /// related to the system mass matrix and the Jacobians of the contact patches contained
305 /// in the vector of RigidConstraintModel named contact_models.
306 ///
307 /// \param[in] model Model of the dynamical system
308 /// \param[in] data Data related to model containing the computed mass matrix and the Jacobian
309 /// of the kinematic tree \param[in] contact_models Vector containing the contact models (which
310 /// frame is in contact and the type of contact: ponctual, 6D rigid, etc.) \param[out]
311 /// contact_datas Vector containing the contact data related to the contact_models. \param[in]
312 /// mus Vector of positive regularization factor allowing to enforce the definite property of
313 /// the KKT matrix.
314 ///
315 /// \remarks The mass matrix and the Jacobians of the dynamical system should have been computed
316 /// first. This can be achieved by simply calling pinocchio::crba.
317 ///
318 template<
319 typename S1,
320 int O1,
321 template<typename, int> class JointCollectionTpl,
322 class ConstraintModelAllocator,
323 class ConstraintDataAllocator,
324 typename VectorLike>
325 void compute(
326 const ModelTpl<S1, O1, JointCollectionTpl> & model,
327 DataTpl<S1, O1, JointCollectionTpl> & data,
328 const std::vector<RigidConstraintModelTpl<S1, O1>, ConstraintModelAllocator> & contact_models,
329 std::vector<RigidConstraintDataTpl<S1, O1>, ConstraintDataAllocator> & contact_datas,
330 const Eigen::MatrixBase<VectorLike> & mus);
331 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
332
333 ///
334 /// \brief Update the damping terms on the upper left block part of the KKT matrix. The damping
335 /// terms should be all positives.
336 ///
337 /// \param[in] mus Vector of positive regularization factor allowing to enforce the definite
338 /// property of the KKT matrix.
339 ///
340 template<typename VectorLike>
341 void updateDamping(const Eigen::MatrixBase<VectorLike> & mus);
342
343 ///
344 /// \brief Update the damping term on the upper left block part of the KKT matrix. The damping
345 /// terms should be all positives.
346 ///
347 /// \param[in] mu Regularization factor allowing to enforce the definite property of the KKT
348 /// matrix.
349 ///
350 void updateDamping(const Scalar & mu);
351
352 /// \brief Size of the decomposition
353 188131 Eigen::DenseIndex size() const
354 {
355 188131 return D.size();
356 }
357
358 /// \brief Returns the total dimension of the constraints contained in the Cholesky
359 /// factorization
360 29898 Eigen::DenseIndex constraintDim() const
361 {
362 29898 return size() - nv;
363 }
364
365 /// \brief Returns the number of contacts associated to this decomposition.
366 ✗ Eigen::DenseIndex numContacts() const
367 {
368 ✗ return num_contacts;
369 }
370
371 ///
372 ///  \brief Computes the solution of \f$ A x = b \f$ where *this is the Cholesky decomposition
373 /// of A.   "in-place" version of ContactCholeskyDecompositionTpl::solve(b) where the
374 /// result is written in b.
375 /// This functions takes as input the vector b, and returns the solution \f$ x = A^-1 b
376 /// \f$.
377 ///
378 /// \param[inout] mat The right-and-side term which also contains the solution of the linear
379 /// system.
380 ///
381 /// \sa ContactCholeskyDecompositionTpl::solve
382 template<typename MatrixLike>
383 void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const;
384
385 ///
386 ///  \brief Computes the solution of \f$ A x = b \f$ where *this is the Cholesky decomposition
387 /// of A.
388 /// This functions takes as input the vector b, and returns the solution \f$ x = A^-1 b
389 /// \f$.
390 ///
391 /// \param[inout] mat The right-and-side term.
392 ///
393 /// \sa ContactCholeskyDecompositionTpl::solveInPlace
394 template<typename MatrixLike>
395 Matrix solve(const Eigen::MatrixBase<MatrixLike> & mat) const;
396
397 ///
398 ///  \brief Retrieves the Cholesky decomposition of the Mass Matrix contained in *this.
399 ///
400 /// \param[in] model Model of the dynamical system.
401 ///
402 template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
403 ContactCholeskyDecompositionTpl
404 getMassMatrixChoeslkyDecomposition(const ModelTpl<S1, O1, JointCollectionTpl> & model) const;
405
406 ///@{
407 /// \brief Vectorwize operations
408 template<typename MatrixLike>
409 void Uv(const Eigen::MatrixBase<MatrixLike> & mat) const;
410
411 template<typename MatrixLike>
412 void Utv(const Eigen::MatrixBase<MatrixLike> & mat) const;
413
414 template<typename MatrixLike>
415 void Uiv(const Eigen::MatrixBase<MatrixLike> & mat) const;
416
417 template<typename MatrixLike>
418 void Utiv(const Eigen::MatrixBase<MatrixLike> & mat) const;
419 ///@}
420
421 /// \brief Returns the matrix resulting from the decomposition
422 Matrix matrix() const;
423
424 /// \brief Fill the input matrix with the matrix resulting from the decomposition
425 template<typename MatrixType>
426 void matrix(const Eigen::MatrixBase<MatrixType> & res) const;
427
428 /// \brief Returns the inverse matrix resulting from the decomposition
429 Matrix inverse() const;
430
431 /// \brief Fill the input matrix with the inverse matrix resulting from the decomposition
432 template<typename MatrixType>
433 void inverse(const Eigen::MatrixBase<MatrixType> & res) const;
434
435 // data
436 Vector D, Dinv;
437 RowMatrix U;
438
439 ///@{
440 /// \brief Friend algorithms
441 template<typename MatrixLike, int ColsAtCompileTime>
442 friend struct details::UvAlgo;
443
444 template<typename MatrixLike, int ColsAtCompileTime>
445 friend struct details::UtvAlgo;
446
447 template<typename MatrixLike, int ColsAtCompileTime>
448 friend struct details::UivAlgo;
449
450 template<typename MatrixLike, int ColsAtCompileTime>
451 friend struct details::UtivAlgo;
452
453 // TODO Remove when API is stabilized
454 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
455 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
456 template<typename Scalar, int Options, typename VectorLike>
457 friend VectorLike & details::inverseAlgo(
458 const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
459 const Eigen::DenseIndex col,
460 const Eigen::MatrixBase<VectorLike> & vec);
461 ///@}
462
463 template<typename S1, int O1>
464 bool operator==(const ContactCholeskyDecompositionTpl<S1, O1> & other) const;
465
466 template<typename S1, int O1>
467 bool operator!=(const ContactCholeskyDecompositionTpl<S1, O1> & other) const;
468 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
469
470 protected:
471 IndexVector parents_fromRow;
472 IndexVector nv_subtree_fromRow;
473
474 ///  \brief Last child of the given joint index
475 IndexVector last_child;
476
477 Vector DUt; // temporary containing the results of D * U^t
478
479 /// \brief Dimension of the tangent of the configuration space of the model
480 Eigen::DenseIndex nv;
481
482 ///  \brief Number of contacts.
483 Eigen::DenseIndex num_contacts;
484
485 VectorOfSliceVector rowise_sparsity_pattern;
486
487 /// \brief Inverse of the top left block of U
488 mutable Matrix U1inv;
489 /// \brief Inverse of the bottom right block of U
490 mutable Matrix U4inv;
491
492 mutable RowMatrix OSIMinv_tmp, Minv_tmp;
493 };
494
495 template<typename ContactCholeskyDecomposition>
496 struct traits<DelassusCholeskyExpressionTpl<ContactCholeskyDecomposition>>
497 {
498 enum
499 {
500 RowsAtCompileTime = Eigen::Dynamic
501 };
502 typedef typename ContactCholeskyDecomposition::Scalar Scalar;
503 typedef typename ContactCholeskyDecomposition::Matrix Matrix;
504 typedef typename ContactCholeskyDecomposition::Vector Vector;
505 };
506
507 template<typename _ContactCholeskyDecomposition>
508 struct DelassusCholeskyExpressionTpl
509 : DelassusOperatorBase<DelassusCholeskyExpressionTpl<_ContactCholeskyDecomposition>>
510 {
511 typedef _ContactCholeskyDecomposition ContactCholeskyDecomposition;
512 typedef typename ContactCholeskyDecomposition::Scalar Scalar;
513 typedef typename ContactCholeskyDecomposition::Vector Vector;
514 typedef typename ContactCholeskyDecomposition::Matrix Matrix;
515 typedef typename ContactCholeskyDecomposition::RowMatrix RowMatrix;
516 typedef DelassusCholeskyExpressionTpl<_ContactCholeskyDecomposition> Self;
517 typedef DelassusOperatorBase<Self> Base;
518
519 typedef
520 typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::Type RowMatrixBlockXpr;
521 typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
522 RowMatrixConstBlockXpr;
523
524 enum
525 {
526 RowsAtCompileTime = traits<DelassusCholeskyExpressionTpl>::RowsAtCompileTime
527 };
528
529 2915 explicit DelassusCholeskyExpressionTpl(const ContactCholeskyDecomposition & self)
530 : Base(self.constraintDim())
531 2915 , self(self)
532 {
533 2915 }
534
535 template<typename MatrixIn, typename MatrixOut>
536 2913 void applyOnTheRight(
537 const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res) const
538 {
539
2/28
✓ Branch 2 taken 2913 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2913 times.
✗ 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 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 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 2913 PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
540
2/28
✓ Branch 2 taken 2913 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2913 times.
✗ 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 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 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 2913 PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), self.constraintDim());
541
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 2913 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 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.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 2913 PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), x.cols());
542
543 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
544
545 const RowMatrixConstBlockXpr U1 =
546
3/6
✓ Branch 1 taken 2913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2913 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2913 times.
✗ Branch 8 not taken.
 2913 self.U.topLeftCorner(self.constraintDim(), self.constraintDim());
547
548
3/4
✓ Branch 2 taken 2913 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2910 times.
✓ Branch 5 taken 3 times.
 2913 if (x.cols() <= self.constraintDim())
549 {
550 RowMatrixBlockXpr tmp_mat =
551
1/2
✓ Branch 2 taken 2910 times.
✗ Branch 3 not taken.
 5820 const_cast<ContactCholeskyDecomposition &>(self).OSIMinv_tmp.topLeftCorner(
552
1/2
✓ Branch 1 taken 2910 times.
✗ Branch 2 not taken.
 2910 self.constraintDim(), x.cols());
553 // tmp_mat.noalias() = U1.adjoint() * x;
554
2/4
✓ Branch 2 taken 2910 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2910 times.
✗ Branch 6 not taken.
 2910 triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
555
7/14
✓ Branch 1 taken 2910 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2910 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2910 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2910 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2910 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2910 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2910 times.
✗ Branch 20 not taken.
 2910 tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
556 // res.const_cast_derived().noalias() = U1 * tmp_mat;
557
1/2
✓ Branch 2 taken 2910 times.
✗ Branch 3 not taken.
 2910 triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
558 }
559 else // do memory allocation
560 {
561
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 RowMatrix tmp_mat(x.rows(), x.cols());
562 // tmp_mat.noalias() = U1.adjoint() * x;
563
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
564
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 3 tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
565 // res.const_cast_derived().noalias() = U1 * tmp_mat;
566
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
567 3 }
568
569 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
570 2913 }
571
572 template<typename MatrixDerived>
573 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
574 4 operator*(const Eigen::MatrixBase<MatrixDerived> & x) const
575 {
576 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
577
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 ReturnType res(self.constraintDim(), x.cols());
578
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 applyOnTheRight(x.derived(), res);
579 4 return res;
580 }
581
582 template<typename MatrixDerived>
583 1 void solveInPlace(const Eigen::MatrixBase<MatrixDerived> & x) const
584 {
585
2/28
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 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 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
586
587 const Eigen::TriangularView<RowMatrixConstBlockXpr, Eigen::UnitUpper> U1 =
588
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 self.U.topLeftCorner(self.constraintDim(), self.constraintDim())
589
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 .template triangularView<Eigen::UnitUpper>();
590
591 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
592
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 U1.solveInPlace(x.const_cast_derived());
593
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 1 x.const_cast_derived().array().colwise() *= -self.Dinv.head(self.constraintDim()).array();
594
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 U1.adjoint().solveInPlace(x);
595 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
596 1 }
597
598 template<typename MatrixDerivedIn, typename MatrixDerivedOut>
599 1 void solve(
600 const Eigen::MatrixBase<MatrixDerivedIn> & x,
601 const Eigen::MatrixBase<MatrixDerivedOut> & res) const
602 {
603 1 res.const_cast_derived() = x;
604 1 solveInPlace(res.const_cast_derived());
605 1 }
606
607 template<typename MatrixDerived>
608 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
609 1 solve(const Eigen::MatrixBase<MatrixDerived> & x) const
610 {
611 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
612
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 ReturnType res(self.constraintDim(), x.cols());
613
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 solve(x.derived(), res);
614 1 return res;
615 }
616
617 /// \brief Returns the Constraint Cholesky decomposition associated to this
618 /// DelassusCholeskyExpression.
619 ✗ const ContactCholeskyDecomposition & cholesky() const
620 {
621 ✗ return self;
622 }
623
624 ✗ Matrix matrix() const
625 {
626 ✗ return self.getInverseOperationalSpaceInertiaMatrix();
627 }
628
629 ✗ Matrix inverse() const
630 {
631 ✗ return self.getOperationalSpaceInertiaMatrix();
632 }
633
634 ///
635 /// \brief Add a damping term to the diagonal of the Delassus matrix. The damping terms should
636 /// be all positives.
637 ///
638 /// \param[in] mus Vector of positive regularization factor allowing to enforce the definite
639 /// positiveness of the matrix.
640 ///
641 template<typename VectorLike>
642 ✗ void updateDamping(const Eigen::MatrixBase<VectorLike> & mus)
643 {
644 ✗ const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mus);
645 }
646
647 ///
648 /// \brief Add a damping term to the diagonal of the Delassus matrix. The damping term should be
649 /// positive.
650 ///
651 /// \param[in] mu Regularization factor allowing to enforce the definite positiveness of the
652 /// matrix.
653 ///
654 1 void updateDamping(const Scalar & mu)
655 {
656 1 const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mu);
657 1 }
658
659 ✗ Eigen::DenseIndex size() const
660 {
661 ✗ return self.constraintDim();
662 }
663 ✗ Eigen::DenseIndex rows() const
664 {
665 ✗ return size();
666 }
667 ✗ Eigen::DenseIndex cols() const
668 {
669 ✗ return size();
670 }
671
672 protected:
673 const ContactCholeskyDecomposition & self;
674 }; // DelassusCholeskyExpression
675
676 } // namespace pinocchio
677
678 // Because of a GCC bug we should NEVER define a function that use ContactCholeskyDecompositionTpl
679 // before doing the explicit template instantiation.
680 // If we don't take care, GCC will not accept any visibility attribute when declaring the
681 // explicit template instantiation of the ContactCholeskyDecompositionTpl class.
682 // The warning message will look like this: type attributes ignored after type is already defined
683 // [-Wattributes] A minimal code example is added on the PR
684 // (https://github.com/stack-of-tasks/pinocchio/pull/2469)
685 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
686 #include "pinocchio/algorithm/contact-cholesky.txx"
687 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
688
689 #include "pinocchio/algorithm/contact-cholesky.hxx"
690
691 #endif // ifndef __pinocchio_algorithm_contact_cholesky_hpp__
692