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 |