cholesky.hpp 8.77 KB
 Florian Valenza committed Apr 29, 2015 1 //  jcarpent committed Apr 03, 2018 2 // Copyright (c) 2015-2018 CNRS  Florian Valenza committed Apr 29, 2015 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // // Pinocchio is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Pinocchio If not, see // .  Nicolas Mansard committed Mar 30, 2015 18 19 20 21 #ifndef __se3_cholesky_hpp__ #define __se3_cholesky_hpp__ #include "pinocchio/multibody/model.hpp"  jcarpent committed Jun 01, 2018 22 #include "pinocchio/multibody/data.hpp"  Nicolas Mansard committed Mar 30, 2015 23 24 25  namespace se3 {  Nicolas Mansard committed Mar 30, 2015 26 27  namespace cholesky {  jcarpent committed Feb 15, 2016 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43  /// /// \brief Compute the Cholesky decomposition of the joint space inertia matrix M contained in data. /// /// \note The Cholesky decomposition corresponds to /// \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. /// /// The result stored in data.U and data.D matrices. One can retrieve the matrice M by performing the /// computation data.U * data.D * data.U.transpose() /// /// See https://en.wikipedia.org/wiki/Cholesky_decomposition for futher details. /// /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. ///  jcarpent committed Mar 16, 2016 44 45  /// \return A reference to the upper triangular matrix \f$U\f$. ///  jcarpent committed Feb 15, 2016 46 47 48  inline const Eigen::MatrixXd & decompose(const Model & model, Data & data);  Nicolas Mansard committed Mar 30, 2015 49   jcarpent committed Feb 15, 2016 50 51 52 53 54 55 56 57 58 59  /// /// \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. /// /// \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$ /// \f$/// M(q) \ddot{q} + b(q, \dot{q}) = \tau \iff \ddot{q} = M(q)^{-1} (\tau - b(q, \dot{q})) /// \f$ /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  jcarpent committed Mar 16, 2016 60  /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$ of the inversion.  jcarpent committed Feb 15, 2016 61  ///  Nicolas Mansard committed Mar 30, 2015 62  template  jcarpent committed Feb 15, 2016 63  Mat & solve(const Model & model, const Data & data,  jcarpent committed Apr 03, 2018 64  const Eigen::MatrixBase & y);  Nicolas Mansard committed Mar 30, 2015 65   jcarpent committed Feb 15, 2016 66  ///  67  /// \brief Performs the multiplication \f$M v \f$ by using the sparsity pattern of the M matrix.  jcarpent committed Feb 15, 2016 68 69 70  /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  71  /// \param[in] min The input matrix to multiply with data.M.  jcarpent committed Feb 15, 2016 72  ///  73  /// \return A the result of \f$Mv \f$.  jcarpent committed Feb 15, 2016 74  ///  Nicolas Mansard committed Mar 30, 2015 75  template  76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109  typename EIGEN_PLAIN_TYPE(Mat) Mv(const Model & model, const Data & data, const Eigen::MatrixBase & min); /// /// \brief Performs the multiplication \f$M v \f$ by using the sparsity pattern of the M matrix. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] min The input matrix to multiply with data.M. /// \param[out] mout The output matrix where the result of \f$Mv \f$ is stored. /// /// \return A reference of the result of \f$Mv \f$. /// template MatRes & Mv(const Model & model, const Data & data, const Eigen::MatrixBase & min, const Eigen::MatrixBase & mout); /// /// \brief Performs the multiplication \f$M v \f$ by using the Cholesky decomposition of M stored in data. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] m The input matrix where the result of \f$Mv \f$ is stored. /// /// \return A reference of the result of \f$Mv \f$. /// template Mat & UDUtv(const Model & model, const Data & data, const Eigen::MatrixBase & m);  jcarpent committed Feb 15, 2016 110   jcarpent committed Feb 15, 2016 111 112 113 114 115  /// /// \brief Perform the sparse multiplication \f$Uv \f$ using the Cholesky decomposition stored in data and acting in place. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  jcarpent committed Mar 16, 2016 116  /// \param[inout] v The input matrix to multiply with data.U and also storing the result.  jcarpent committed Feb 15, 2016 117 118 119  /// /// \return A reference to the result of \f$Uv \f$ stored in v. ///  Nicolas Mansard committed Mar 30, 2015 120  template  jcarpent committed Feb 15, 2016 121 122  Mat & Uv(const Model & model, const Data & data,  jcarpent committed Apr 03, 2018 123  const Eigen::MatrixBase & v);  jcarpent committed Feb 15, 2016 124   jcarpent committed Feb 15, 2016 125 126 127 128 129  /// /// \brief Perform the sparse multiplication \f$U^{\top}v \f$ using the Cholesky decomposition stored in data and acting in place. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  jcarpent committed Mar 16, 2016 130  /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result.  jcarpent committed Feb 15, 2016 131 132 133  /// /// \return A reference to the result of \f$U^{\top}v \f$ stored in v. ///  Nicolas Mansard committed Mar 30, 2015 134  template  jcarpent committed Feb 15, 2016 135 136  Mat & Utv(const Model & model, const Data & data,  jcarpent committed Apr 03, 2018 137  const Eigen::MatrixBase & v);  jcarpent committed Feb 15, 2016 138   jcarpent committed Feb 15, 2016 139 140 141 142 143  /// /// \brief Perform the pivot inversion \f$U^{-1}v \f$ using the Cholesky decomposition stored in data and acting in place. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  jcarpent committed Mar 16, 2016 144  /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result.  jcarpent committed Feb 15, 2016 145 146 147 148 149  /// /// \return A reference to the result of \f$U^{-1}v \f$ stored in v. /// /// \remark The result is similar to the code data.U.triangularView ().solveInPlace(v). ///  Nicolas Mansard committed Mar 30, 2015 150  template  jcarpent committed Feb 15, 2016 151 152  Mat & Uiv(const Model & model, const Data & data ,  jcarpent committed Apr 03, 2018 153  const Eigen::MatrixBase & v);  jcarpent committed Feb 15, 2016 154   jcarpent committed Feb 15, 2016 155 156 157 158 159  /// /// \brief Perform the pivot inversion \f$U^{-\top}v \f$ using the Cholesky decomposition stored in data and acting in place. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  jcarpent committed Mar 16, 2016 160  /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result.  jcarpent committed Feb 15, 2016 161 162 163 164 165  /// /// \return A reference to the result of \f$U^{-\top}v \f$ stored in v. /// /// \remark The result is similar to the code data.U.triangularView ().transpose().solveInPlace(v). ///  Nicolas Mansard committed Mar 30, 2015 166  template  jcarpent committed Feb 15, 2016 167 168  Mat & Utiv(const Model & model, const Data & data ,  jcarpent committed Apr 03, 2018 169  const Eigen::MatrixBase & v);  jcarpent committed Feb 15, 2016 170   jcarpent committed Feb 15, 2016 171 172 173 174 175  /// /// \brief Perform the sparse inversion \f$M^{-1}v \f$ using the Cholesky decomposition stored in data and acting in place. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.  jcarpent committed Mar 16, 2016 176  /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result.  jcarpent committed Feb 15, 2016 177 178 179  /// /// \return A reference to the result of \f$M^{-1}v \f$ stored in v. ///  180  template  jcarpent committed Feb 15, 2016 181 182  Mat & solve(const Model & model, const Data & data ,  jcarpent committed Apr 03, 2018 183  const Eigen::MatrixBase & v);  jcarpent committed Apr 03, 2018 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198  /// /// \brief PComputes the inverse of the joint inertia matrix M from its Cholesky factorization. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[out] Minv The output matrix where the result is stored. /// /// \return A reference to the result. /// template Mat & computeMinv(const Model & model, const Data & data, const Eigen::MatrixBase & Minv);  jcarpent committed Feb 15, 2016 199  } // namespace cholesky  Nicolas Mansard committed Mar 30, 2015 200 } // namespace se3  jcarpent committed Feb 15, 2016 201 202 203 204 205 206  /* --- Details -------------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/algorithm/cholesky.hxx"  Nicolas Mansard committed Mar 30, 2015 207 #endif // ifndef __se3_cholesky_hpp__