\f$\begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ + /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$
+ /// where \f$\dot{q}^{-} \f$ is the generalized velocity before impact, + /// \f$M \f$ is the joint space mass matrix, \f$J \f$ the constraint Jacobian and \f$\epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact). + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType Type of the joint velocity vector. + /// \tparam ConstraintMatrixType Type of the constraint matrix. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration (vector dim model.nq). + /// \param[in] v_before The joint velocity before impact (vector dim model.nv). + /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). + /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1]. + /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. + /// + /// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector. + /// + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType> + inline const typename DataTpl::TangentVectorType & + impulseDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v_before, + const Eigen::MatrixBase & J, + const Scalar r_coeff = 0., + const Scalar inv_damping = 0.); + + /// + /// \brief Compute the impulse dynamics with contact constraints. + /// \note It computes the following problem:
\f$\begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ + /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$
+ /// where \f$\dot{q}^{-} \f$ is the generalized velocity before impact, + /// \f$M \f$ is the joint space mass matrix, \f$J \f$ the constraint Jacobian and \f$\epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact). + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType Type of the joint velocity vector. + /// \tparam ConstraintMatrixType Type of the constraint matrix. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] v_before The joint velocity before impact (vector dim model.nv). + /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). + /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1]. + /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. + /// + /// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector. + /// + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType> + inline const typename DataTpl::TangentVectorType & + impulseDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & v_before, + const Eigen::MatrixBase & J, + const Scalar r_coeff = 0., + const Scalar inv_damping = 0.); + /// /// \brief Compute the impulse dynamics with contact constraints. /// \note It computes the following problem: