Template Function pinocchio::computeDampedDelassusMatrixInverse

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename ConfigVectorType, class ModelAllocator, class DataAllocator, typename MatrixType>
void pinocchio::computeDampedDelassusMatrixInverse(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, DataTpl<Scalar, Options, JointCollectionTpl> &data, const Eigen::MatrixBase<ConfigVectorType> &q, const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> &contact_models, std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> &contact_data, const Eigen::MatrixBase<MatrixType> &damped_delassus_inverse, const Scalar mu, const bool scaled = false, const bool Pv = true)

Computes the inverse of the Delassus matrix associated to a set of given constraints.

Note

A hint: a typical value for mu is 1e-4 when two contact constraints or more are redundant.

Template Parameters:
  • JointCollection – Collection of Joint types.

  • ConfigVectorType – Type of the joint configuration vector.

  • ModelAllocator – Allocator class for the std::vector.

  • DataAllocator – Allocator class for the std::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 (size model.nq).

  • contact_models[in] Vector of contact models.

  • contact_datas[in] Vector of contact data.

  • damped_delassus_inverse[out] The resulting damped Delassus matrix.

  • mu[in] Damping factor well-posdnessed of the problem.

  • scaled[in] If set to true, the solution is scaled my a factor μ to avoid numerical rounding issues.

  • Pv[in] If set to true, uses PV-OSIMr, otherwise uses EFPA.

Returns:

The damped inverse Delassus matrix.