pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
contact-cholesky.hpp
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
15namespace 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
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
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;
103
107 ContactCholeskyDecompositionTpl() = default;
108
114 template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
115 explicit ContactCholeskyDecompositionTpl(const ModelTpl<S1, O1, JointCollectionTpl> & model)
116 {
117 // TODO Remove when API is stabilized
118 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
119 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
120 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models;
121 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
122 allocate(model, empty_contact_models);
123 }
124
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
146 ContactCholeskyDecompositionTpl(const ContactCholeskyDecompositionTpl & copy) = default;
147
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
168 Matrix getInverseOperationalSpaceInertiaMatrix() const
169 {
170 Matrix res(constraintDim(), constraintDim());
171 getInverseOperationalSpaceInertiaMatrix(res);
172 return res;
173 }
174
175 template<typename MatrixType>
176 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 const ConstBlockXpr U1 = U.topLeftCorner(constraintDim(), constraintDim());
182
183 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
184 MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res);
185 OSIMinv_tmp.noalias() = D.head(constraintDim()).asDiagonal() * U1.adjoint();
186 res_.noalias() = -U1 * OSIMinv_tmp;
187 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
188 }
189
192 DelassusCholeskyExpression getDelassusCholeskyExpression() const
193 {
194 return DelassusCholeskyExpression(*this);
195 }
196
200 Matrix getOperationalSpaceInertiaMatrix() const
201 {
202 Matrix res(constraintDim(), constraintDim());
203 getOperationalSpaceInertiaMatrix(res);
204 return res;
205 }
206
207 template<typename MatrixType>
208 void getOperationalSpaceInertiaMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
209 {
210 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 U.topLeftCorner(constraintDim(), constraintDim())
216 .template triangularView<Eigen::UnitUpper>();
217
218 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
219 U1inv.setIdentity();
220 U1.solveInPlace(U1inv); // TODO: implement Sparse Inverse
221 OSIMinv_tmp.noalias() = -U1inv.adjoint() * Dinv.head(constraintDim()).asDiagonal();
222 res.noalias() = OSIMinv_tmp * U1inv;
223 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
224 }
225
226 Matrix getInverseMassMatrix() const
227 {
228 Matrix res(nv, nv);
229 getInverseMassMatrix(res);
230 return res;
231 }
232
233 template<typename MatrixType>
234 void getInverseMassMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
235 {
236 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 U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
242
243 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
244 U4inv.setIdentity();
245 U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
246 Minv_tmp.noalias() = U4inv.adjoint() * Dinv.tail(nv).asDiagonal();
247 res.noalias() = Minv_tmp * U4inv;
248 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
249 }
250
251 template<typename MatrixType>
252 void getJMinv(const Eigen::MatrixBase<MatrixType> & res_) const
253 {
254 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 U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
259 ConstBlockXpr U2 = U.topRightCorner(constraintDim(), nv);
260 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
261 U4inv.setIdentity();
262 U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
263 res.noalias() = U2 * U4inv;
264 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
265 }
266
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 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 compute(model, data, contact_models, contact_datas, Vector::Constant(U1inv.rows(), mu));
300 }
301
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
340 template<typename VectorLike>
341 void updateDamping(const Eigen::MatrixBase<VectorLike> & mus);
342
350 void updateDamping(const Scalar & mu);
351
353 Eigen::DenseIndex size() const
354 {
355 return D.size();
356 }
357
360 Eigen::DenseIndex constraintDim() const
361 {
362 return size() - nv;
363 }
364
366 Eigen::DenseIndex numContacts() const
367 {
368 return num_contacts;
369 }
370
382 template<typename MatrixLike>
383 void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const;
384
394 template<typename MatrixLike>
395 Matrix solve(const Eigen::MatrixBase<MatrixLike> & mat) const;
396
402 template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
403 ContactCholeskyDecompositionTpl
404 getMassMatrixChoeslkyDecomposition(const ModelTpl<S1, O1, JointCollectionTpl> & model) const;
405
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;
420
422 Matrix matrix() const;
423
425 template<typename MatrixType>
426 void matrix(const Eigen::MatrixBase<MatrixType> & res) const;
427
429 Matrix inverse() const;
430
432 template<typename MatrixType>
433 void inverse(const Eigen::MatrixBase<MatrixType> & res) const;
434
435 // data
436 Vector D, Dinv;
437 RowMatrix U;
438
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);
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
475 IndexVector last_child;
476
477 Vector DUt; // temporary containing the results of D * U^t
478
480 Eigen::DenseIndex nv;
481
483 Eigen::DenseIndex num_contacts;
484
485 VectorOfSliceVector rowise_sparsity_pattern;
486
488 mutable Matrix U1inv;
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 explicit DelassusCholeskyExpressionTpl(const ContactCholeskyDecomposition & self)
530 : Base(self.constraintDim())
531 , self(self)
532 {
533 }
534
535 template<typename MatrixIn, typename MatrixOut>
536 void applyOnTheRight(
537 const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res) const
538 {
539 PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
540 PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), self.constraintDim());
541 PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), x.cols());
542
543 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
544
545 const RowMatrixConstBlockXpr U1 =
546 self.U.topLeftCorner(self.constraintDim(), self.constraintDim());
547
548 if (x.cols() <= self.constraintDim())
549 {
550 RowMatrixBlockXpr tmp_mat =
551 const_cast<ContactCholeskyDecomposition &>(self).OSIMinv_tmp.topLeftCorner(
552 self.constraintDim(), x.cols());
553 // tmp_mat.noalias() = U1.adjoint() * x;
554 triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
555 tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
556 // res.const_cast_derived().noalias() = U1 * tmp_mat;
557 triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
558 }
559 else // do memory allocation
560 {
561 RowMatrix tmp_mat(x.rows(), x.cols());
562 // tmp_mat.noalias() = U1.adjoint() * x;
563 triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
564 tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
565 // res.const_cast_derived().noalias() = U1 * tmp_mat;
566 triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
567 }
568
569 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
570 }
571
572 template<typename MatrixDerived>
573 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
574 operator*(const Eigen::MatrixBase<MatrixDerived> & x) const
575 {
576 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
577 ReturnType res(self.constraintDim(), x.cols());
578 applyOnTheRight(x.derived(), res);
579 return res;
580 }
581
582 template<typename MatrixDerived>
583 void solveInPlace(const Eigen::MatrixBase<MatrixDerived> & x) const
584 {
585 PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
586
587 const Eigen::TriangularView<RowMatrixConstBlockXpr, Eigen::UnitUpper> U1 =
588 self.U.topLeftCorner(self.constraintDim(), self.constraintDim())
589 .template triangularView<Eigen::UnitUpper>();
590
591 PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
592 U1.solveInPlace(x.const_cast_derived());
593 x.const_cast_derived().array().colwise() *= -self.Dinv.head(self.constraintDim()).array();
594 U1.adjoint().solveInPlace(x);
595 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
596 }
597
598 template<typename MatrixDerivedIn, typename MatrixDerivedOut>
599 void solve(
600 const Eigen::MatrixBase<MatrixDerivedIn> & x,
601 const Eigen::MatrixBase<MatrixDerivedOut> & res) const
602 {
603 res.const_cast_derived() = x;
604 solveInPlace(res.const_cast_derived());
605 }
606
607 template<typename MatrixDerived>
608 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
609 solve(const Eigen::MatrixBase<MatrixDerived> & x) const
610 {
611 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
612 ReturnType res(self.constraintDim(), x.cols());
613 solve(x.derived(), res);
614 return res;
615 }
616
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
641 template<typename VectorLike>
642 void updateDamping(const Eigen::MatrixBase<VectorLike> & mus)
643 {
644 const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mus);
645 }
646
654 void updateDamping(const Scalar & mu)
655 {
656 const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mu);
657 }
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__
pinocchio::cholesky::Utiv
Mat & Utiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
pinocchio::cholesky::Uiv
Mat & Uiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
pinocchio::cholesky::solve
Mat & solve(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &y)
Return the solution of using the Cholesky decomposition stored in data given the entry ....
pinocchio::cholesky::Uv
Mat & Uv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the sparse multiplication using the Cholesky decomposition stored in data and acting in plac...
pinocchio::cholesky::Utv
Mat & Utv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the sparse multiplication using the Cholesky decomposition stored in data and acting in plac...
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
pinocchio::copy
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
Definition copy.hpp:42
pinocchio::DelassusCholeskyExpressionTpl::updateDamping
void updateDamping(const Scalar &mu)
Add a damping term to the diagonal of the Delassus matrix. The damping term should be positive.
Definition contact-cholesky.hpp:654
pinocchio::DelassusCholeskyExpressionTpl::updateDamping
void updateDamping(const Eigen::MatrixBase< VectorLike > &mus)
Add a damping term to the diagonal of the Delassus matrix. The damping terms should be all positives.
Definition contact-cholesky.hpp:642
pinocchio::DelassusCholeskyExpressionTpl::cholesky
const ContactCholeskyDecomposition & cholesky() const
Returns the Constraint Cholesky decomposition associated to this DelassusCholeskyExpression.
Definition contact-cholesky.hpp:619
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72