crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
math.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2019, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
10 #define CROCODDYL_CORE_UTILS_MATH_HPP_
11 
12 #include <Eigen/Dense>
13 #include <algorithm>
14 #include <limits>
15 
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);
19  double tolerance =
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)
24  .matrix()
25  .asDiagonal() *
26  svd.matrixU().adjoint();
27 }
28 
29 #endif // CROCODDYL_CORE_UTILS_MATH_HPP_