Template Function pinocchio::contactInverseDynamics

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, class ConstraintModelAllocator, class ConstraintDataAllocator, class CoulombFrictionConelAllocator, typename VectorLikeR, typename VectorLikeGamma, typename VectorLikeLam>
const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &pinocchio::contactInverseDynamics(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, Scalar dt, const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &contact_models, std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> &contact_datas, const std::vector<CoulombFrictionConeTpl<Scalar>, CoulombFrictionConelAllocator> &cones, const Eigen::MatrixBase<VectorLikeR> &R, const Eigen::MatrixBase<VectorLikeGamma> &constraint_correction, ProximalSettingsTpl<Scalar> &settings, const boost::optional<VectorLikeLam> &lambda_guess = boost::none)

The Contact Inverse Dynamics algorithm. It computes the inverse dynamics in the presence of contacts, aka the joint torques according to the current state of the system and the desired joint accelerations.

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

  • dt[in] The time step.

  • contact_models[in] The list of contact models.

  • contact_datas[in] The list of contact_datas.

  • cones[in] list of friction cones.

  • R[in] vector representing the diagonal of the compliance matrix.

  • constraint_correction[in] vector representing the constraint correction.

  • settings[in] The settings for the proximal algorithm.

  • lambda_guess[in] initial guess for the contact forces.

Returns:

The desired joint torques stored in data.tau.