9 #ifndef CROCODDYL_CORE_UTILS_MATH_HPP_ 10 #define CROCODDYL_CORE_UTILS_MATH_HPP_ 12 #include <Eigen/Dense> 16 template <
typename MatrixType>
17 MatrixType pseudoInverse(
const MatrixType& a,
double epsilon = std::numeric_limits<double>::epsilon()) {
18 Eigen::JacobiSVD<MatrixType> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
20 epsilon *
static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
21 return svd.matrixV() *
22 (svd.singularValues().array().abs() > tolerance)
23 .select(svd.singularValues().array().inverse(), 0)
26 svd.matrixU().adjoint();
29 #endif // CROCODDYL_CORE_UTILS_MATH_HPP_