Template Function pinocchio::getCentroidalDynamicsDerivatives

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3>
inline void pinocchio::getCentroidalDynamicsDerivatives(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, DataTpl<Scalar, Options, JointCollectionTpl> &data, const Eigen::MatrixBase<Matrix6xLike1> &dh_dq, const Eigen::MatrixBase<Matrix6xLike1> &dhdot_dq, const Eigen::MatrixBase<Matrix6xLike2> &dhdot_dv, const Eigen::MatrixBase<Matrix6xLike3> &dhdot_da)

Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives. pinocchio::computeRNEADerivatives should have been called first.

Computes the first order approximation of the centroidal dynamics time derivative and corresponds to the following equation \( 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} \)

Parameters:

  • model – [in] The model structure of the rigid body system.

  • data – [in] The data structure of the rigid body system.

  • dhdot_dq – [out] The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv).

  • dhdot_dv – [out] The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv).

  • dhdot_da – [out] The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv).

Returns:

It also computes the current centroidal dynamics and its time derivative. For information, the centroidal momentum matrix is equivalent to dhdot_da.