crba.hpp 3.52 KB
 Florian Valenza committed Apr 29, 2015 1 //  jcarpent committed Jun 01, 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 // .  18 #ifndef __se3_crba_hpp__  Nicolas Mansard committed Mar 30, 2015 19 20 21 #define __se3_crba_hpp__ #include "pinocchio/multibody/model.hpp"  jcarpent committed Jun 01, 2018 22 #include "pinocchio/multibody/data.hpp"  23 #include "pinocchio/algorithm/check.hpp"  Nicolas Mansard committed Mar 30, 2015 24 25 26  namespace se3 {  jcarpent committed Jan 27, 2016 27 28 29  /// /// \brief Computes the upper triangular part of the joint space inertia matrix M by /// using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008).  Valenza Florian committed Apr 08, 2016 30  /// The result is accessible through data.M.  jcarpent committed Jan 27, 2016 31 32 33 34 35 36 37 38 39 40 41 42 43 44  /// /// \note You can easly get data.M symetric by copying the stricly upper trinangular part /// in the stricly lower tringular part with /// data.M.triangularView() = data.M.transpose().triangularView(); /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration vector (dim model.nq). /// /// \return The joint space inertia matrix with only the upper triangular part computed. /// inline const Eigen::MatrixXd & crba(const Model & model, Data & data,  Nicolas Mansard committed Mar 30, 2015 45  const Eigen::VectorXd & q);  jcarpent committed Mar 06, 2016 46   jcarpent committed Sep 26, 2017 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64  /// /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta /// according to the current joint configuration and velocity. /// /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration vector (dim model.nq). /// \param[in] v The joint velocity vector (dim model.nv). /// /// \return The Centroidal Momentum Matrix Ag. /// inline const Data::Matrix6x & ccrba(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v);  jcarpent committed Mar 06, 2016 65  ///  jcarpent committed Sep 28, 2017 66  /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.  jcarpent committed Mar 06, 2016 67  ///  jcarpent committed Sep 28, 2017 68  /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$\dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.  jcarpent committed Mar 06, 2016 69 70 71 72 73 74  /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration vector (dim model.nq). /// \param[in] v The joint configuration vector (dim model.nv). ///  jcarpent committed Sep 28, 2017 75  /// \return The Centroidal Momentum Matrix time derivative dAg  jcarpent committed Mar 06, 2016 76 77  /// inline const Data::Matrix6x &  jcarpent committed Sep 28, 2017 78  dccrba(const Model & model,  jcarpent committed Mar 06, 2016 79 80 81  Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v);  Nicolas Mansard committed Mar 30, 2015 82   83 84  DEFINE_ALGO_CHECKER(CRBA);  Nicolas Mansard committed Mar 30, 2015 85 86 87 } // namespace se3 /* --- Details -------------------------------------------------------------------- */  jcarpent committed Apr 10, 2016 88 #include "pinocchio/algorithm/crba.hxx"  Nicolas Mansard committed Mar 30, 2015 89 90  #endif // ifndef __se3_crba_hpp__