Template Function pinocchio::centerOfMass(const ModelTpl<Scalar, Options, JointCollectionTpl>&, DataTpl<Scalar, Options, JointCollectionTpl>&, const Eigen::MatrixBase<ConfigVectorType>&, const Eigen::MatrixBase<TangentVectorType>&, const bool)

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 &pinocchio::centerOfMass(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, DataTpl<Scalar, Options, JointCollectionTpl> &data, const Eigen::MatrixBase<ConfigVectorType> &q, const Eigen::MatrixBase<TangentVectorType> &v, const bool computeSubtreeComs = true)

Computes the center of mass position and velocity of a given model according to a particular joint configuration and velocity. The result is accessible through data.com[0], data.vcom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).

Template Parameters:
  • JointCollection – Collection of Joint types.

  • ConfigVectorType – Type of the joint configuration vector.

  • TangentVectorType – Type of the joint velocity 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).

  • computeSubtreeComs[in] If true, the algorithm computes also the center of mass of the subtrees.

Returns:

The center of mass position of the full rigid body system expressed in the world frame.