se3::cholesky Namespace Reference

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.

Function Documentation

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]modelThe model structure of the rigid body system.
[in]dataThe 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::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.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
inout]v The input matrix to multiply with data.M and storing the result.
[in]usingCholeskyIf true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree.
Returns:
A reference to the result of \( Mv \).
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]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
inout]y The input matrix to inverse which also contains the result \(x\) of the inversion.
[in]modelThe model structure of the rigid body system.
[in]dataThe 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().

template<typename Mat >
Mat& se3::cholesky::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.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
inout]v The input matrix to multiply with data.U^{-1} and also storing the result.
Returns:
A reference to the result of \( U^{-1}v \) stored in v.
Remarks:
The result is similar to the code data.U.triangularView<Eigen::Upper> ().solveInPlace(v).

Referenced by se3::forwardDynamics(), and se3::impulseDynamics().

template<typename Mat >
Mat& se3::cholesky::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.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
inout]v The input matrix to multiply with data.U^{-} and also storing the result.
Returns:
A reference to the result of \( U^{-\top}v \) stored in v.
Remarks:
The result is similar to the code data.U.triangularView<Eigen::Upper> ().transpose().solveInPlace(v).
template<typename Mat >
Mat& se3::cholesky::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.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
inout]v The input matrix to multiply with data.U.tranpose() and also storing the result.
Returns:
A reference to the result of \( U^{\top}v \) stored in v.
template<typename Mat >
Mat& se3::cholesky::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.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
inout]v The input matrix to multiply with data.U and also storing the result.
Returns:
A reference to the result of \( Uv \) stored in v.