$darkmode
pinocchio 4.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
cholesky.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4 
5 #pragma once
6 
7 // IWYU pragma: begin_keep
8 #include <cassert>
9 #include <cstddef>
10 #include <vector>
11 #include <algorithm>
12 
13 #include <Eigen/Core>
14 
15 #include "pinocchio/macros.hpp"
16 #include "pinocchio/eigen-common.hpp"
17 
18 #include "pinocchio/multibody.hpp"
19 
20 #include "pinocchio/algorithm/check-model.hpp"
21 // IWYU pragma: end_keep
22 
23 namespace pinocchio
24 {
25  namespace cholesky
26  {
27 
48  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
49  inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & decompose(
50  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
51  DataTpl<Scalar, Options, JointCollectionTpl> & data);
52 
71  template<
72  typename Scalar,
73  int Options,
74  template<typename, int> class JointCollectionTpl,
75  typename Mat>
76  Mat & solve(
77  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
78  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
79  const Eigen::MatrixBase<Mat> & y);
80 
93  template<
94  typename Scalar,
95  int Options,
96  template<typename, int> class JointCollectionTpl,
97  typename Mat>
98  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(
99  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
100  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
101  const Eigen::MatrixBase<Mat> & min);
102 
116  template<
117  typename Scalar,
118  int Options,
119  template<typename, int> class JointCollectionTpl,
120  typename Mat,
121  typename MatRes>
122  MatRes & Mv(
123  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
124  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
125  const Eigen::MatrixBase<Mat> & min,
126  const Eigen::MatrixBase<MatRes> & mout);
127 
140  template<
141  typename Scalar,
142  int Options,
143  template<typename, int> class JointCollectionTpl,
144  typename Mat>
145  Mat & UDUtv(
146  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
147  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
148  const Eigen::MatrixBase<Mat> & m);
149 
162  template<
163  typename Scalar,
164  int Options,
165  template<typename, int> class JointCollectionTpl,
166  typename Mat>
167  Mat & Uv(
168  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
169  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
170  const Eigen::MatrixBase<Mat> & v);
171 
185  template<
186  typename Scalar,
187  int Options,
188  template<typename, int> class JointCollectionTpl,
189  typename Mat>
190  Mat & Utv(
191  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
192  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
193  const Eigen::MatrixBase<Mat> & v);
194 
210  template<
211  typename Scalar,
212  int Options,
213  template<typename, int> class JointCollectionTpl,
214  typename Mat>
215  Mat & Uiv(
216  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
217  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
218  const Eigen::MatrixBase<Mat> & v);
219 
236  template<
237  typename Scalar,
238  int Options,
239  template<typename, int> class JointCollectionTpl,
240  typename Mat>
241  Mat & Utiv(
242  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
243  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
244  const Eigen::MatrixBase<Mat> & v);
245 
258  template<
259  typename Scalar,
260  int Options,
261  template<typename, int> class JointCollectionTpl,
262  typename Mat>
263  Mat & computeMinv(
264  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
265  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
266  const Eigen::MatrixBase<Mat> & Minv);
267 
280  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
281  const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinv(
282  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
283  DataTpl<Scalar, Options, JointCollectionTpl> & data)
284  {
285  return computeMinv(model, data, data.Minv);
286  }
287 
288  } // namespace cholesky
289 } // namespace pinocchio
290 
291 /* --- Details -------------------------------------------------------------------- */
292 /* --- Details -------------------------------------------------------------------- */
293 /* --- Details -------------------------------------------------------------------- */
294 // IWYU pragma: begin_exports
295 #include "pinocchio/src/algorithm/cholesky.hxx"
296 // IWYU pragma: end_exports
pinocchio::cholesky::UDUtv
Mat & UDUtv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &m)
Performs the multiplication by using the Cholesky decomposition of M stored in data.
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::decompose
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
pinocchio::cholesky::computeMinv
Mat & computeMinv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &Minv)
Computes the inverse of the joint space inertia matrix M from its Cholesky factorization.
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::Mv
Mat Mv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &min)
Performs the multiplication by using the sparsity pattern of the M matrix.
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11