pinocchio  3.3.1
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 #ifndef __pinocchio_cholesky_hpp__
6 #define __pinocchio_cholesky_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 namespace pinocchio
12 {
13  namespace cholesky
14  {
15 
36  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
37  inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & decompose(
38  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
39  DataTpl<Scalar, Options, JointCollectionTpl> & data);
40 
59  template<
60  typename Scalar,
61  int Options,
62  template<typename, int> class JointCollectionTpl,
63  typename Mat>
64  Mat & solve(
65  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
66  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
67  const Eigen::MatrixBase<Mat> & y);
68 
81  template<
82  typename Scalar,
83  int Options,
84  template<typename, int> class JointCollectionTpl,
85  typename Mat>
86  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(
87  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
88  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
89  const Eigen::MatrixBase<Mat> & min);
90 
104  template<
105  typename Scalar,
106  int Options,
107  template<typename, int> class JointCollectionTpl,
108  typename Mat,
109  typename MatRes>
110  MatRes & Mv(
111  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
112  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
113  const Eigen::MatrixBase<Mat> & min,
114  const Eigen::MatrixBase<MatRes> & mout);
115 
128  template<
129  typename Scalar,
130  int Options,
131  template<typename, int> class JointCollectionTpl,
132  typename Mat>
133  Mat & UDUtv(
134  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
135  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
136  const Eigen::MatrixBase<Mat> & m);
137 
150  template<
151  typename Scalar,
152  int Options,
153  template<typename, int> class JointCollectionTpl,
154  typename Mat>
155  Mat & Uv(
156  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
157  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
158  const Eigen::MatrixBase<Mat> & v);
159 
173  template<
174  typename Scalar,
175  int Options,
176  template<typename, int> class JointCollectionTpl,
177  typename Mat>
178  Mat & Utv(
179  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
180  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
181  const Eigen::MatrixBase<Mat> & v);
182 
198  template<
199  typename Scalar,
200  int Options,
201  template<typename, int> class JointCollectionTpl,
202  typename Mat>
203  Mat & Uiv(
204  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
205  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
206  const Eigen::MatrixBase<Mat> & v);
207 
224  template<
225  typename Scalar,
226  int Options,
227  template<typename, int> class JointCollectionTpl,
228  typename Mat>
229  Mat & Utiv(
230  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
231  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
232  const Eigen::MatrixBase<Mat> & v);
233 
246 
247  // TODO Clearify, it seems it is exactly the same as solve in l. 54
248  template<
249  typename Scalar,
250  int Options,
251  template<typename, int> class JointCollectionTpl,
252  typename Mat>
253  Mat & solve(
254  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
255  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
256  const Eigen::MatrixBase<Mat> & v);
257 
270  template<
271  typename Scalar,
272  int Options,
273  template<typename, int> class JointCollectionTpl,
274  typename Mat>
275  Mat & computeMinv(
276  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
277  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
278  const Eigen::MatrixBase<Mat> & Minv);
279 
292  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
293  const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinv(
294  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
295  DataTpl<Scalar, Options, JointCollectionTpl> & data)
296  {
297  return computeMinv(model, data, data.Minv);
298  }
299 
300  } // namespace cholesky
301 } // namespace pinocchio
302 
303 /* --- Details -------------------------------------------------------------------- */
304 /* --- Details -------------------------------------------------------------------- */
305 /* --- Details -------------------------------------------------------------------- */
306 #include "pinocchio/algorithm/cholesky.hxx"
307 
308 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
309  #include "pinocchio/algorithm/cholesky.txx"
310 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
311 
312 #endif // ifndef __pinocchio_cholesky_hpp__
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::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::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::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
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:202