crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
math.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 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
20template <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
39template <typename MatrixLike>
40struct 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
49template <typename MatrixLike>
50MatrixLike 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_