centroidal-derivatives.hpp 5.56 KB
 Justin Carpentier committed Oct 29, 2018 1 2 3 4 // // Copyright (c) 2018 INRIA //  Justin Carpentier committed Nov 08, 2018 5 6 #ifndef __pinocchio_algorithm_centroidal_derivatives_hpp__ #define __pinocchio_algorithm_centroidal_derivatives_hpp__  Justin Carpentier committed Oct 29, 2018 7 8 9 10 11  #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" #include "pinocchio/algorithm/check.hpp"  Justin Carpentier committed Nov 08, 2018 12 namespace pinocchio  Justin Carpentier committed Oct 29, 2018 13 14 15 16 17 18 { /// /// \brief Computes the analytical derivatives of the centroidal dynamics with respect to /// the joint configuration vector, velocity and acceleration. ///  Justin Carpentier committed Oct 29, 2018 19 20 21  /// \details Computes the first order approximation of the centroidal dynamics time derivative /// and corresponds to the following equation /// \f$ Gabriele Buondonno committed Dec 01, 2018 22 23 24  /// d\dot{h_{g}} = \frac{\partial \dot{h_{g}}}{\partial \mathbf{q}} d\mathbf{q} /// + \frac{\partial \dot{h_{g}}}{\partial \mathbf{v}} d\mathbf{v} /// + \frac{\partial \dot{h_{g}}}{\partial \mathbf{a}} d\mathbf{a}  Justin Carpentier committed Oct 29, 2018 25 26  /// \f$ ///  Justin Carpentier committed Oct 29, 2018 27 28  /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector.  Justin Carpentier committed Oct 29, 2018 29 30  /// \tparam TangentVectorType1 Type of the joint velocity vector. /// \tparam TangentVectorType2 Type of the joint acceleration vector.  Justin Carpentier committed Oct 29, 2018 31 32 33 34 35 36  /// /// \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). /// \param[in] a The joint acceleration vector (dim model.nv).  37  /// \param[out] dh_dq The partial derivative of the centroidal momentum with respect to the configuration vector (dim 6 x model.nv).  Justin Carpentier committed Oct 29, 2018 38 39 40 41  /// \param[out] dhdot_dq The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv). /// \param[out] dhdot_dv The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv). /// \param[out] dhdot_da The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv). ///  Justin Carpentier committed Oct 29, 2018 42 43 44  /// \result It also computes the current centroidal dynamics and its time derivative. /// For information, the centroidal momentum matrix is equivalent to dhdot_da. ///  Justin Carpentier committed Oct 29, 2018 45 46  template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,  47  typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3>  Justin Carpentier committed Oct 29, 2018 48 49 50 51 52 53  inline void computeCentroidalDynamicsDerivatives(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & a,  54  const Eigen::MatrixBase & dh_dq,  Justin Carpentier committed Oct 29, 2018 55 56 57  const Eigen::MatrixBase & dhdot_dq, const Eigen::MatrixBase & dhdot_dv, const Eigen::MatrixBase & dhdot_da);  Justin Carpentier committed Oct 29, 2018 58 59  /// /// \brief Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives.  Justin Carpentier committed Nov 08, 2018 60  /// pinocchio::computeRNEADerivatives should have been called first.  Justin Carpentier committed Oct 29, 2018 61 62 63 64  /// /// \details Computes the first order approximation of the centroidal dynamics time derivative /// and corresponds to the following equation /// \f$ Gabriele Buondonno committed Dec 01, 2018 65 66 67  /// d\dot{h_{g}} = \frac{\partial \dot{h_{g}}}{\partial \mathbf{q}} d\mathbf{q} /// + \frac{\partial \dot{h_{g}}}{\partial \mathbf{v}} d\mathbf{v} /// + \frac{\partial \dot{h_{g}}}{\partial \mathbf{a}} d\mathbf{a}  Justin Carpentier committed Oct 29, 2018 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86  /// \f$ /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[out] dhdot_dq The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv). /// \param[out] dhdot_dv The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv). /// \param[out] dhdot_da The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv). /// /// \result It also computes the current centroidal dynamics and its time derivative. /// For information, the centroidal momentum matrix is equivalent to dhdot_da. /// template class JointCollectionTpl, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> inline void getCentroidalDynamicsDerivatives(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & dhdot_dq, const Eigen::MatrixBase & dhdot_dv, const Eigen::MatrixBase & dhdot_da);  Justin Carpentier committed Oct 29, 2018 87 88   Justin Carpentier committed Nov 08, 2018 89 } // namespace pinocchio  Justin Carpentier committed Oct 29, 2018 90 91 92 93  /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/algorithm/centroidal-derivatives.hxx"  Justin Carpentier committed Nov 08, 2018 94 #endif // ifndef __pinocchio_algorithm_centroidal_derivatives_hpp__