GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: include/pinocchio/algorithm/cholesky.hpp Lines: 2 2 100.0 % Date: 2024-04-26 13:14:21 Branches: 0 0 - %

 Line Branch Exec Source 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 16  /// 17  /// \brief Compute the Cholesky decomposition of the joint space inertia matrix M contained in data. 18  /// 19  /// \note The Cholesky decomposition corresponds to 20  /// \f$M = U D U^{\top}\f$ with \f$U\f$ an upper triangular matrix with ones on its main diagonal and \f$D\f$ a diagonal matrix. 21  /// 22  /// The result stored in data.U and data.D matrices. One can retrieve the matrice M by performing the 23  /// computation data.U * data.D * data.U.transpose() 24  /// 25  /// See https://en.wikipedia.org/wiki/Cholesky_decomposition for futher details. 26  /// 27  /// \tparam JointCollection Collection of Joint types. 28  /// 29  /// \param[in] model The model structure of the rigid body system. 30  /// \param[in] data The data structure of the rigid body system. 31  /// 32  /// \return A reference to the upper triangular matrix \f$U\f$. 33  /// 34  template class JointCollectionTpl> 35  inline const typename DataTpl::MatrixXs & 36  decompose(const ModelTpl & model, 37  DataTpl & data); 38 39  /// 40  /// \brief Return the solution \f$x\f$ of \f$M x = y \f$ using the Cholesky decomposition stored in data given the entry \f$y \f$. Act like solveInPlace of Eigen::LLT. 41  /// 42  /// \note This algorithm is useful to compute the forward dynamics, retriving the joint acceleration \f$\ddot{q} \f$ from the current joint torque \f$\tau \f$ 43  /// \f$ 44  /// M(q) \ddot{q} + b(q, \dot{q}) = \tau \iff \ddot{q} = M(q)^{-1} (\tau - b(q, \dot{q})) 45  /// \f$ 46  /// 47  /// \tparam JointCollection Collection of Joint types. 48  /// 49  /// \param[in] model The model structure of the rigid body system. 50  /// \param[in] data The data structure of the rigid body system. 51  /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$ of the inversion. 52  /// 53  template class JointCollectionTpl, typename Mat> 54  Mat & solve(const ModelTpl & model, 55  const DataTpl & data, 56  const Eigen::MatrixBase & y); 57 58  /// 59  /// \brief Performs the multiplication \f$M v \f$ by using the sparsity pattern of the M matrix. 60  /// 61  /// \tparam JointCollection Collection of Joint types. 62  /// 63  /// \param[in] model The model structure of the rigid body system. 64  /// \param[in] data The data structure of the rigid body system. 65  /// \param[in] min The input matrix to multiply with data.M. 66  /// 67  /// \return A the result of \f$Mv \f$. 68  /// 69  template class JointCollectionTpl, typename Mat> 70  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) 71  Mv(const ModelTpl & model, 72  const DataTpl & data, 73  const Eigen::MatrixBase & min); 74 75  /// 76  /// \brief Performs the multiplication \f$M v \f$ by using the sparsity pattern of the M matrix. 77  /// 78  /// \tparam JointCollection Collection of Joint types. 79  /// 80  /// \param[in] model The model structure of the rigid body system. 81  /// \param[in] data The data structure of the rigid body system. 82  /// \param[in] min The input matrix to multiply with data.M. 83  /// \param[out] mout The output matrix where the result of \f$Mv \f$ is stored. 84  /// 85  /// \return A reference of the result of \f$Mv \f$. 86  /// 87  template class JointCollectionTpl, typename Mat, typename MatRes> 88  MatRes & Mv(const ModelTpl & model, 89  const DataTpl & data, 90  const Eigen::MatrixBase & min, 91  const Eigen::MatrixBase & mout); 92 93 94  /// 95  /// \brief Performs the multiplication \f$M v \f$ by using the Cholesky decomposition of M stored in data. 96  /// 97  /// \tparam JointCollection Collection of Joint types. 98  /// 99  /// \param[in] model The model structure of the rigid body system. 100  /// \param[in] data The data structure of the rigid body system. 101  /// \param[inout] m The input matrix where the result of \f$Mv \f$ is stored. 102  /// 103  /// \return A reference of the result of \f$Mv \f$. 104  /// 105  template class JointCollectionTpl, typename Mat> 106  Mat & UDUtv(const ModelTpl & model, 107  const DataTpl & data, 108  const Eigen::MatrixBase & m); 109 110  /// 111  /// \brief Perform the sparse multiplication \f$Uv \f$ using the Cholesky decomposition stored in data and acting in place. 112  /// 113  /// \tparam JointCollection Collection of Joint types. 114  /// 115  /// \param[in] model The model structure of the rigid body system. 116  /// \param[in] data The data structure of the rigid body system. 117  /// \param[inout] v The input matrix to multiply with data.U and also storing the result. 118  /// 119  /// \return A reference to the result of \f$Uv \f$ stored in v. 120  /// 121  template class JointCollectionTpl, typename Mat> 122  Mat & Uv(const ModelTpl & model, 123  const DataTpl & data, 124  const Eigen::MatrixBase & v); 125 126  /// 127  /// \brief Perform the sparse multiplication \f$U^{\top}v \f$ using the Cholesky decomposition stored in data and acting in place. 128  /// 129  /// \tparam JointCollection Collection of Joint types. 130  /// 131  /// \param[in] model The model structure of the rigid body system. 132  /// \param[in] data The data structure of the rigid body system. 133  /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result. 134  /// 135  /// \return A reference to the result of \f$U^{\top}v \f$ stored in v. 136  /// 137  template class JointCollectionTpl, typename Mat> 138  Mat & Utv(const ModelTpl & model, 139  const DataTpl & data, 140  const Eigen::MatrixBase & v); 141 142  /// 143  /// \brief Perform the pivot inversion \f$U^{-1}v \f$ using the Cholesky decomposition stored in data and acting in place. 144  /// 145  /// \tparam JointCollection Collection of Joint types. 146  /// 147  /// \param[in] model The model structure of the rigid body system. 148  /// \param[in] data The data structure of the rigid body system. 149  /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result. 150  /// 151  /// \return A reference to the result of \f$U^{-1}v \f$ stored in v. 152  /// 153  /// \remark The result is similar to the code data.U.triangularView ().solveInPlace(v). 154  /// 155  template class JointCollectionTpl, typename Mat> 156  Mat & Uiv(const ModelTpl & model, 157  const DataTpl & data , 158  const Eigen::MatrixBase & v); 159 160  /// 161  /// \brief Perform the pivot inversion \f$U^{-\top}v \f$ using the Cholesky decomposition stored in data and acting in place. 162  /// 163  /// \tparam JointCollection Collection of Joint types. 164  /// 165  /// \param[in] model The model structure of the rigid body system. 166  /// \param[in] data The data structure of the rigid body system. 167  /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result. 168  /// 169  /// \return A reference to the result of \f$U^{-\top}v \f$ stored in v. 170  /// 171  /// \remark The result is similar to the code data.U.triangularView ().transpose().solveInPlace(v). 172  /// 173  template class JointCollectionTpl, typename Mat> 174  Mat & Utiv(const ModelTpl & model, 175  const DataTpl & data , 176  const Eigen::MatrixBase & v); 177 178  /// 179  /// \brief Perform the sparse inversion \f$M^{-1}v \f$ using the Cholesky decomposition stored in data and acting in place. 180  /// 181  /// \tparam JointCollection Collection of Joint types. 182  /// 183  /// \param[in] model The model structure of the rigid body system. 184  /// \param[in] data The data structure of the rigid body system. 185  /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result. 186  /// 187  /// \return A reference to the result of \f$M^{-1}v \f$ stored in v. 188  /// 189  template class JointCollectionTpl, typename Mat> 190  Mat & solve(const ModelTpl & model, 191  const DataTpl & data , 192  const Eigen::MatrixBase & v); 193 194  /// 195  /// \brief Computes the inverse of the joint space inertia matrix M from its Cholesky factorization. 196  /// 197  /// \tparam JointCollection Collection of Joint types. 198  /// 199  /// \param[in] model The model structure of the rigid body system. 200  /// \param[in] data The data structure of the rigid body system. 201  /// \param[out] Minv The output matrix where the result is stored. 202  /// 203  /// \return A reference to the result. 204  /// 205  template class JointCollectionTpl, typename Mat> 206  Mat & computeMinv(const ModelTpl & model, 207  const DataTpl & data, 208  const Eigen::MatrixBase & Minv); 209 210  /// 211  /// \brief Computes the inverse of the joint space inertia matrix M from its Cholesky factorization. 212  /// The results is then directly stored in data.Minv. 213  /// 214  /// \tparam JointCollection Collection of Joint types. 215  /// 216  /// \param[in] model The model structure of the rigid body system. 217  /// \param[in] data The data structure of the rigid body system. 218  /// 219  /// \return A reference to the result data.Minv. 220  /// 221  template class JointCollectionTpl> 222  const typename DataTpl::RowMatrixXs & 223 1  computeMinv(const ModelTpl & model, 224  DataTpl & data) 225  { 226 1  return computeMinv(model,data,data.Minv); 227  } 228 229  } // namespace cholesky 230 } // namespace pinocchio 231 232 /* --- Details -------------------------------------------------------------------- */ 233 /* --- Details -------------------------------------------------------------------- */ 234 /* --- Details -------------------------------------------------------------------- */ 235 #include "pinocchio/algorithm/cholesky.hxx" 236 237 #endif // ifndef __pinocchio_cholesky_hpp__

 Generated by: GCOVR (Version 4.2)