Template Function pinocchio::computeCentroidalDynamicsDerivatives

Function Documentation

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

Computes the analytical derivatives of the centroidal dynamics with respect to the joint configuration vector, velocity and acceleration.

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} \)

Template Parameters:

  • JointCollection – Collection of Joint types.

  • ConfigVectorType – Type of the joint configuration vector.

  • TangentVectorType1 – Type of the joint velocity vector.

  • TangentVectorType2 – Type of the joint acceleration vector.

Parameters:

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

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

  • q – [in] The joint configuration vector (dim model.nq).

  • v – [in] The joint velocity vector (dim model.nv).

  • a – [in] The joint acceleration vector (dim model.nv).

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

  • 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.