Template Function pinocchio::constraintDynamics(const ModelTpl<Scalar, Options, JointCollectionTpl>&, DataTpl<Scalar, Options, JointCollectionTpl>&, const Eigen::MatrixBase<ConfigVectorType>&, const Eigen::MatrixBase<TangentVectorType1>&, const Eigen::MatrixBase<TangentVectorType2>&, const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator>&, std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator>&, ProximalSettingsTpl<Scalar>&)

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, class ConstraintModelAllocator, class ConstraintDataAllocator>
inline const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &pinocchio::constraintDynamics(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> &tau, const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &contact_models, std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> &contact_datas, ProximalSettingsTpl<Scalar> &settings)

Computes the forward dynamics with contact constraints according to a given list of contact information.

It computes the following problem: [

minq¨q¨q¨freeM(q)s.t.J(q)q¨+γ(q,q˙)=0
where q¨free is the free acceleration (i.e. without constraints), M is the mass matrix, J the constraint Jacobian and γ is the constraint drift. By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.

Note

When using forwardDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm.

Note

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

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 torque vector.

  • Allocator – 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).

  • v[in] The joint velocity (size model.nv).

  • tau[in] The joint torque vector (size model.nv).

  • contact_models[in] Vector of contact models.

  • contact_datas[in] Vector of contact data.

  • settings[in] Proximal settings (mu, accuracy and maximal number of iterations).

Returns:

A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.