Template Function pinocchio::computeContactImpulses
Defined in File contact-inverse-dynamics.hpp
Function Documentation
-
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename VectorLikeC, class ConstraintModelAllocator, class ConstraintDataAllocator, class CoulombFrictionConelAllocator, typename VectorLikeR, typename VectorLikeGamma, typename VectorLikeImp>
const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &pinocchio::computeContactImpulses(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, DataTpl<Scalar, Options, JointCollectionTpl> &data, const Eigen::MatrixBase<VectorLikeC> &c_ref, 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<VectorLikeImp> &impulse_guess = boost::none) Compute the contact impulses given a target velocity of contact points.
- 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.
c_ref – [in] The contact point velocity
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.
impulse_guess – [in] initial guess for the contact impulses.
- Returns:
The desired joint torques stored in data.tau.