pinocchio  3.2.0
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()
108  {
109  }
110 
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 
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 
148  ContactCholeskyDecompositionTpl(const ContactCholeskyDecompositionTpl & copy) = default;
149 
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 
170  Matrix getInverseOperationalSpaceInertiaMatrix() const
171  {
172  Matrix res(constraintDim(), constraintDim());
173  getInverseOperationalSpaceInertiaMatrix(res);
174  return res;
175  }
176 
177  template<typename MatrixType>
178  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  const ConstBlockXpr U1 = U.topLeftCorner(constraintDim(), constraintDim());
184 
185  PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
186  MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res);
187  OSIMinv_tmp.noalias() = D.head(constraintDim()).asDiagonal() * U1.adjoint();
188  res_.noalias() = -U1 * OSIMinv_tmp;
189  PINOCCHIO_EIGEN_MALLOC_ALLOWED();
190  }
191 
194  DelassusCholeskyExpression getDelassusCholeskyExpression() const
195  {
196  return DelassusCholeskyExpression(*this);
197  }
198 
202  Matrix getOperationalSpaceInertiaMatrix() const
203  {
204  Matrix res(constraintDim(), constraintDim());
205  getOperationalSpaceInertiaMatrix(res);
206  return res;
207  }
208 
209  template<typename MatrixType>
210  void getOperationalSpaceInertiaMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
211  {
212  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  U.topLeftCorner(constraintDim(), constraintDim())
218  .template triangularView<Eigen::UnitUpper>();
219 
220  PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
221  U1inv.setIdentity();
222  U1.solveInPlace(U1inv); // TODO: implement Sparse Inverse
223  OSIMinv_tmp.noalias() = -U1inv.adjoint() * Dinv.head(constraintDim()).asDiagonal();
224  res.noalias() = OSIMinv_tmp * U1inv;
225  PINOCCHIO_EIGEN_MALLOC_ALLOWED();
226  }
227 
228  Matrix getInverseMassMatrix() const
229  {
230  Matrix res(nv, nv);
231  getInverseMassMatrix(res);
232  return res;
233  }
234 
235  template<typename MatrixType>
236  void getInverseMassMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
237  {
238  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  U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
244 
245  PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
246  U4inv.setIdentity();
247  U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
248  Minv_tmp.noalias() = U4inv.adjoint() * Dinv.tail(nv).asDiagonal();
249  res.noalias() = Minv_tmp * U4inv;
250  PINOCCHIO_EIGEN_MALLOC_ALLOWED();
251  }
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 
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  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  compute(model, data, contact_models, contact_datas, Vector::Constant(U1inv.rows(), mu));
303  }
304 
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 
344  template<typename VectorLike>
345  void updateDamping(const Eigen::MatrixBase<VectorLike> & mus);
346 
354  void updateDamping(const Scalar & mu);
355 
357  Eigen::DenseIndex size() const
358  {
359  return D.size();
360  }
361 
364  Eigen::DenseIndex constraintDim() const
365  {
366  return size() - nv;
367  }
368 
370  Eigen::DenseIndex numContacts() const
371  {
372  return num_contacts;
373  }
374 
386  template<typename MatrixLike>
387  void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const;
388 
398  template<typename MatrixLike>
399  Matrix solve(const Eigen::MatrixBase<MatrixLike> & mat) const;
400 
406  template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
407  ContactCholeskyDecompositionTpl
408  getMassMatrixChoeslkyDecomposition(const ModelTpl<S1, O1, JointCollectionTpl> & model) const;
409 
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;
424 
426  Matrix matrix() const;
427 
429  template<typename MatrixType>
430  void matrix(const Eigen::MatrixBase<MatrixType> & res) const;
431 
433  Matrix inverse() const;
434 
436  template<typename MatrixType>
437  void inverse(const Eigen::MatrixBase<MatrixType> & res) const;
438 
439  // data
440  Vector D, Dinv;
441  RowMatrix U;
442 
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);
466 
467  template<typename S1, int O1>
468  bool operator==(const ContactCholeskyDecompositionTpl<S1, O1> & other) const
469  {
470  bool is_same = true;
471 
472  if (nv != other.nv || num_contacts != other.num_contacts)
473  return false;
474 
475  if (
476  D.size() != other.D.size() || Dinv.size() != other.Dinv.size() || U.rows() != other.U.rows()
477  || U.cols() != other.U.cols())
478  return false;
479 
480  is_same &= (D == other.D);
481  is_same &= (Dinv == other.Dinv);
482  is_same &= (U == other.U);
483 
484  is_same &= (parents_fromRow == other.parents_fromRow);
485  is_same &= (nv_subtree_fromRow == other.nv_subtree_fromRow);
486  is_same &= (last_child == other.last_child);
487  // is_same &= (rowise_sparsity_pattern == other.rowise_sparsity_pattern);
488 
489  return is_same;
490  }
491 
492  template<typename S1, int O1>
493  bool operator!=(const ContactCholeskyDecompositionTpl<S1, O1> & other) const
494  {
495  return !(*this == other);
496  }
497  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
498 
499  protected:
500  IndexVector parents_fromRow;
501  IndexVector nv_subtree_fromRow;
502 
504  IndexVector last_child;
505 
506  Vector DUt; // temporary containing the results of D * U^t
507 
509  Eigen::DenseIndex nv;
510 
512  Eigen::DenseIndex num_contacts;
513 
514  VectorOfSliceVector rowise_sparsity_pattern;
515 
517  mutable Matrix U1inv;
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  explicit DelassusCholeskyExpressionTpl(const ContactCholeskyDecomposition & self)
559  : Base(self.constraintDim())
560  , self(self)
561  {
562  }
563 
564  template<typename MatrixIn, typename MatrixOut>
565  void applyOnTheRight(
566  const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res) const
567  {
568  PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
569  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), self.constraintDim());
570  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), x.cols());
571 
572  PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
573 
574  const RowMatrixConstBlockXpr U1 =
575  self.U.topLeftCorner(self.constraintDim(), self.constraintDim());
576 
577  if (x.cols() <= self.constraintDim())
578  {
579  RowMatrixBlockXpr tmp_mat =
580  const_cast<ContactCholeskyDecomposition &>(self).OSIMinv_tmp.topLeftCorner(
581  self.constraintDim(), x.cols());
582  // tmp_mat.noalias() = U1.adjoint() * x;
583  triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
584  tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
585  // res.const_cast_derived().noalias() = U1 * tmp_mat;
586  triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
587  }
588  else // do memory allocation
589  {
590  RowMatrix tmp_mat(x.rows(), x.cols());
591  // tmp_mat.noalias() = U1.adjoint() * x;
592  triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
593  tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
594  // res.const_cast_derived().noalias() = U1 * tmp_mat;
595  triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
596  }
597 
598  PINOCCHIO_EIGEN_MALLOC_ALLOWED();
599  }
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 
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 
670  template<typename VectorLike>
671  void updateDamping(const Eigen::MatrixBase<VectorLike> & mus)
672  {
673  const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mus);
674  }
675 
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__
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:683
pinocchio::DelassusCholeskyExpressionTpl::cholesky
const ContactCholeskyDecomposition & cholesky() const
Returns the Constraint Cholesky decomposition associated to this DelassusCholeskyExpression.
Definition: contact-cholesky.hpp:648
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:671
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72