Functions |
const Eigen::MatrixXd & | decompose (const Model &model, Data &data) |
| Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
|
template<typename Mat > |
Mat & | solve (const Model &model, const Data &data, Eigen::MatrixBase< Mat > &y) |
| Return the solution \(x\) of \( M x = y \) using the Cholesky decomposition stored in data given the entry \( y \).
|
template<typename Mat > |
Mat & | Mv (const Model &model, const Data &data, Eigen::MatrixBase< Mat > &v, const bool usingCholesky=false) |
| Perform the multiplication \( M v \) either by using computed Cholesky decomposition or from raw computation.
|
template<typename Mat > |
Mat & | Uv (const Model &model, const Data &data, Eigen::MatrixBase< Mat > &v) |
| Perform the sparse multiplication \( Uv \) using the Cholesky decomposition stored in data and acting in place.
|
template<typename Mat > |
Mat & | Utv (const Model &model, const Data &data, Eigen::MatrixBase< Mat > &v) |
| Perform the sparse multiplication \( U^{\top}v \) using the Cholesky decomposition stored in data and acting in place.
|
template<typename Mat > |
Mat & | Uiv (const Model &model, const Data &data, Eigen::MatrixBase< Mat > &v) |
| Perform the pivot inversion \( U^{-1}v \) using the Cholesky decomposition stored in data and acting in place.
|
template<typename Mat > |
Mat & | Utiv (const Model &model, const Data &data, Eigen::MatrixBase< Mat > &v) |
| Perform the pivot inversion \( U^{-\top}v \) using the Cholesky decomposition stored in data and acting in place.
|
const Eigen::MatrixXd& se3::cholesky::decompose |
( |
const Model & |
model, |
|
|
Data & |
data |
|
) |
| [inline] |
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
- Note:
- The Cholesky decomposition corresponds to \( M = U D U^{\top}\) with \(U\) an upper triangular matrix with ones on its main diagonal and \(D\) a diagonal matrix.
The result stored in data.U and data.D matrices. One can retrieve the matrice M by performing the computation data.U * data.D * data.U.transpose()
See https://en.wikipedia.org/wiki/Cholesky_decomposition for futher details.
- Parameters:
-
[in] | model | The model structure of the rigid body system. |
[in] | data | The data structure of the rigid body system. |
- Returns:
- A reference to the upper triangular matrix \(U\).
Referenced by se3::forwardDynamics(), and se3::impulseDynamics().
template<typename Mat >
Mat & se3::cholesky::solve |
( |
const Model & |
model, |
|
|
const Data & |
data, |
|
|
Eigen::MatrixBase< Mat > & |
y |
|
) |
| |
Return the solution \(x\) of \( M x = y \) using the Cholesky decomposition stored in data given the entry \( y \).
Perform the sparse inversion \( M^{-1}v \) using the Cholesky decomposition stored in data and acting in place.
Act like solveInPlace of Eigen::LLT.
- 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})) \)
- Parameters:
-
[in] | model | The model structure of the rigid body system. |
[in] | data | The data structure of the rigid body system. |
| inout] | y The input matrix to inverse which also contains the result \(x\) of the inversion. |
[in] | model | The model structure of the rigid body system. |
[in] | data | The data structure of the rigid body system. |
| inout] | v 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.
Referenced by se3::forwardDynamics(), and se3::impulseDynamics().