Commit c237917d by jcarpent

### [C++] Add impulse dynamics algorithm

parent 869abf64
 ... ... @@ -21,6 +21,7 @@ #include "pinocchio/multibody/model.hpp" #include "pinocchio/simulation/compute-all-terms.hpp" #include "pinocchio/algorithm/cholesky.hpp" #include "pinocchio/algorithm/crba.hpp" #include namespace se3 ... ... @@ -93,6 +94,67 @@ namespace se3 return a; } /// /// \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). /// /// \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 /// \param[in] updateKinematics If true, the algorithm calls first se3::crba. Otherwise, it uses the current mass matrix value stored in data. /// /// \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. /// inline const Eigen::VectorXd & impulseDynamics(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v_before, const Eigen::MatrixXd & J, const double r_coeff = 0., const bool updateKinematics = true ) { assert(q.size() == model.nq); assert(v_before.size() == model.nv); assert(J.cols() == model.nv); Eigen::VectorXd & impulse_c = data.impulse_c; Eigen::VectorXd & 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); data.sDUiJt = J.transpose(); // Compute U^-1 * J.T cholesky::Uiv(model, data, data.sDUiJt); for(int k=0;k
 ... ... @@ -482,7 +482,7 @@ namespace se3 /// \brief Cholesky decompostion of \JMinvJt. Eigen::LLT llt_JMinvJt; /// \brief Lagrange Multipliers corresponding to contact forces. /// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics. Eigen::VectorXd lambda_c; /// \brief Temporary corresponding to \f$\sqrt{D} U^{-1} J^{\top} \f$. ... ... @@ -491,6 +491,12 @@ namespace se3 /// \brief Temporary corresponding to the residual torque \f$\tau - b(q,\dot{q}) \f$. Eigen::VectorXd torque_residual; /// \brief Generalized velocity after impact. Eigen::VectorXd dq_after; /// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics. Eigen::VectorXd impulse_c; /// /// \brief Default constructor of se3::Data from a se3::Model. /// ... ...
 ... ... @@ -289,6 +289,8 @@ namespace se3 ,lambda_c() ,sDUiJt(ref.nv,ref.nv) ,torque_residual(ref.nv) ,dq_after(model.nv) ,impulse_c() { /* Create data strcture associated to the joints */ for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!