pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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 
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 
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::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::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::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::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
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::PINOCCHIO_UNSUPPORTED_MESSAGE
struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") ContactCholeskyDecompositionTpl
Contact Cholesky decomposition structure. This structure allows to compute in a efficient and parsimo...
Definition: contact-cholesky.hpp:55
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::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
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::cholesky
const ContactCholeskyDecomposition & cholesky() const
Returns the Constraint Cholesky decomposition associated to this DelassusCholeskyExpression.
Definition: contact-cholesky.hpp:619
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::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72