Commit 506ef5e3 by Justin Carpentier

parent c10eedde
 ... ... @@ -35,7 +35,7 @@ namespace pinocchio /// \param[in] tau The joint torque vector (dim model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). /// \param[in] gamma The drift of the constraints (dim nb_constraints). /// \param[in] inv_damping Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. /// \param[in] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. /// \param[in] updateKinematics If true, the algorithm calls first pinocchio::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\ /// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet. /// ... ... @@ -72,6 +72,70 @@ namespace pinocchio const Eigen::MatrixBase & J, const Eigen::MatrixBase & MJtJ_inv); /// /// \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] 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] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1]. /// /// \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 inv_damping, const Scalar r_coeff); /// /// \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] inv_damping Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank. /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1]. /// /// \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 inv_damping, const Scalar r_coeff); /// /// \brief Compute the impulse dynamics with contact constraints. /// \note It computes the following problem:
... ... @@ -96,6 +160,7 @@ namespace pinocchio /// \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> PINOCCHIO_DEPRECATED inline const typename DataTpl::TangentVectorType & impulseDynamics(const ModelTpl & model, DataTpl & data, ... ...
 ... ... @@ -109,10 +109,26 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v_before, const Eigen::MatrixBase & J, const Scalar r_coeff, const bool updateKinematics) const Scalar inv_damping, const Scalar r_coeff) { assert(q.size() == model.nq); // Compute the mass matrix crba(model, data, q); return impulseDynamics(model,data,v_before,J,inv_damping,r_coeff); } template class JointCollectionTpl, 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 inv_damping, const Scalar r_coeff) { assert(v_before.size() == model.nv); assert(J.cols() == model.nv); assert(model.check(data) && "data is not consistent with model."); ... ... @@ -122,10 +138,6 @@ namespace pinocchio typename Data::VectorXs & impulse_c = data.impulse_c; typename Data::TangentVectorType & dq_after = data.dq_after; // Compute the mass matrix if (updateKinematics) crba(model, data, q); // Compute the UDUt decomposition of data.M cholesky::decompose(model, data); ... ... @@ -135,6 +147,8 @@ namespace pinocchio for(int k=0;k class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType> PINOCCHIO_DEPRECATED 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, const bool updateKinematics) { if(updateKinematics) return impulseDynamics(model,data,q,v_before,J,Scalar(0),r_coeff); else return impulseDynamics(model,data,v_before,J,Scalar(0),r_coeff); } } // namespace pinocchio #endif // ifndef __pinocchio_contact_dynamics_hxx__
 ... ... @@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix) //Check Impulse Dynamics const double r_coeff = 1.; VectorXd v_before = VectorXd::Ones(model.nv); pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, true); pinocchio::impulseDynamics(model, data, q, v_before, J, 0., r_coeff); data.M.triangularView() = data.M.transpose().triangularView(); MJtJ << data.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12); ... ... @@ -234,7 +234,7 @@ BOOST_AUTO_TEST_CASE ( test_ID ) Eigen::MatrixXd H(J.transpose()); pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, true); pinocchio::impulseDynamics(model, data, q, v_before, J, 0., r_coeff); data.M.triangularView() = data.M.transpose().triangularView(); MatrixXd Minv (data.M.inverse()); ... ...
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!