crocoddyl  1.5.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 <boost/type_traits.hpp>
13 
14 #include <Eigen/Dense>
15 #include <algorithm>
16 #include <limits>
17 
18 // fwd
19 
20 template <typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
22  typedef typename MatrixLike::Scalar Scalar;
23  typedef typename MatrixLike::RealScalar RealScalar;
24 
25  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a, const RealScalar& epsilon) {
26  using std::max;
27  Eigen::JacobiSVD<MatrixLike> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
28  RealScalar tolerance =
29  epsilon * static_cast<Scalar>(max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
30  return svd.matrixV() *
31  (svd.singularValues().array().abs() > tolerance)
32  .select(svd.singularValues().array().inverse(), 0)
33  .matrix()
34  .asDiagonal() *
35  svd.matrixU().adjoint();
36  }
37 };
38 
39 template <typename MatrixLike>
40 struct pseudoInverseAlgo<MatrixLike, false> {
41  typedef typename MatrixLike::Scalar Scalar;
42  typedef typename MatrixLike::RealScalar RealScalar;
43 
44  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a, const RealScalar&) {
45  return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
46  }
47 };
48 
49 template <typename MatrixLike>
50 MatrixLike pseudoInverse(const Eigen::MatrixBase<MatrixLike>& a,
51  const typename MatrixLike::RealScalar& epsilon =
52  Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
53  return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
54 }
55 
56 #endif // CROCODDYL_CORE_UTILS_MATH_HPP_