static Eigen::MatrixXd crba_proxy(const ModelHandler& model,
                                   DataHandler & data,
                                   const VectorXd_fx & q)
 {
   data->M.fill(0);
   crba(*model,*data,q);
   data->M.triangularView<Eigen::StrictlyLower>()
   = data->M.transpose().triangularView<Eigen::StrictlyLower>();
   return data->M;
 }
Beispiel #2
0
 ///
 /// \brief Compute the impulse dynamics with contact constraints.
 /// \note It computes the following problem: <BR>
 ///       <CENTER> \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$ </CENTER> <BR>
 ///       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. Must be in [0;1].
 /// \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<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
   
   data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
   data.llt_JMinvJt.compute(data.JMinvJt);
   
   // Compute the Lagrange Multipliers related to the contact impulses
   impulse_c = (-r_coeff - 1.) * (J * v_before);
   data.llt_JMinvJt.solveInPlace (impulse_c);
   
   // Compute the joint velocity after impacts
   dq_after = J.transpose() * impulse_c;
   cholesky::solve (model, data, dq_after);
   dq_after += v_before;
   
   return dq_after;
 }