Template Function pinocchio::cholesky::solve

Function Documentation

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename Mat>
Mat &pinocchio::cholesky::solve(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, const DataTpl<Scalar, Options, JointCollectionTpl> &data, const Eigen::MatrixBase<Mat> &y)

Return the solution \(x\) of \( M x = y \) using the Cholesky decomposition stored in data given the entry \( y \). Act like solveInPlace of Eigen::LLT.

Perform the sparse inversion \( M^{-1}v \) using the Cholesky decomposition stored in data and acting in place.

Note

This algorithm is useful to compute the forward dynamics, retriving the joint acceleration \( \ddot{q} \) from the current joint torque \( \tau \)\( M(q) \ddot{q} + b(q, \dot{q}) = \tau \iff \ddot{q} = M(q)^{-1} (\tau - b(q, \dot{q})) \)

Template Parameters:

  • JointCollection – Collection of Joint types.

  • JointCollection – Collection of Joint types.

Parameters:

  • model – [in] The model structure of the rigid body system.

  • data – [in] The data structure of the rigid body system.

  • y – [inout] The input matrix to inverse which also contains the result \(x\) of the inversion.

  • model – [in] The model structure of the rigid body system.

  • data – [in] The data structure of the rigid body system.

  • v – [inout] The input matrix to multiply with data.M^{-1} and also storing the result.

Returns:

A reference to the result of \( M^{-1}v \) stored in v.