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

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)

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.

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.

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.