GCC Code Coverage Report

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