GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/utils/math.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 22 0.0%
Branches: 0 44 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
10 #define CROCODDYL_CORE_UTILS_MATH_HPP_
11
12 #include <Eigen/Cholesky>
13 #include <Eigen/Dense>
14 #include <algorithm>
15 #include <cmath>
16 #include <limits>
17 #include <type_traits>
18
19 #include "crocoddyl/core/utils/scalar.hpp"
20
21 #ifdef CROCODDYL_WITH_CODEGEN
22 #include <cppad/cg/support/cppadcg_eigen.hpp>
23 #include <cppad/cppad.hpp>
24 #endif
25
26 namespace crocoddyl {
27
28 template <typename Scalar>
29 ✗ constexpr Scalar pi() {
30 ✗ return static_cast<Scalar>(3.14159265358979323846264338327950288);
31 }
32
33 template <typename Scalar>
34 typename std::enable_if<std::is_floating_point<Scalar>::value, bool>::type
35 ✗ isfinite(const Scalar& value) {
36 ✗ return std::isfinite(value);
37 }
38
39 template <typename MatrixLike,
40 bool value =
41 (Eigen::NumTraits<typename MatrixLike::Scalar>::IsInteger == 0)>
42 struct pseudoInverseAlgo {
43 typedef typename MatrixLike::Scalar Scalar;
44 typedef typename MatrixLike::RealScalar RealScalar;
45
46 ✗ static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
47 const RealScalar& epsilon) {
48 using std::max;
49 ✗ Eigen::JacobiSVD<MatrixLike> svd(a,
50 Eigen::ComputeThinU | Eigen::ComputeThinV);
51 ✗ RealScalar tolerance = epsilon *
52 ✗ static_cast<Scalar>(max(a.cols(), a.rows())) *
53 ✗ svd.singularValues().array().abs()(0);
54 // FIX: Replace select() with a lambda function
55 Eigen::Matrix<typename MatrixLike::Scalar, Eigen::Dynamic, 1>
56 ✗ invSingularValues =
57 ✗ svd.singularValues().unaryExpr([&](const Scalar& x) {
58 ✗ return (x > tolerance) ? Scalar(1) / x : Scalar(0);
59 });
60 ✗ return svd.matrixV() * invSingularValues.asDiagonal() *
61 ✗ svd.matrixU().adjoint();
62 }
63 };
64
65 template <typename MatrixLike>
66 struct pseudoInverseAlgo<MatrixLike, false> {
67 typedef typename MatrixLike::Scalar Scalar;
68 typedef typename MatrixLike::RealScalar RealScalar;
69
70 static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
71 const RealScalar&) {
72 return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
73 }
74 };
75
76 template <typename MatrixLike>
77 ✗ MatrixLike pseudoInverse(
78 const Eigen::MatrixBase<MatrixLike>& a,
79 const typename MatrixLike::RealScalar& epsilon =
80 Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
81 ✗ return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
82 }
83
84 template <typename Scalar>
85 typename std::enable_if<std::is_floating_point<Scalar>::value, bool>::type
86 ✗ checkPSD(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& a) {
87 Eigen::SelfAdjointEigenSolver<
88 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>
89 ✗ eig(a);
90 ✗ if (eig.info() != Eigen::Success ||
91 ✗ eig.eigenvalues().minCoeff() < ScaleNumerics<Scalar>(-1e-9)) {
92 ✗ return false;
93 }
94 ✗ return true;
95 }
96
97 template <typename Scalar>
98 typename std::enable_if<!std::is_floating_point<Scalar>::value, bool>::type
99 checkPSD(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&) {
100 // Do nothing for AD types
101 return true;
102 }
103
104 } // namespace crocoddyl
105
106 #ifdef CROCODDYL_WITH_CODEGEN
107 template <typename Scalar>
108 bool isfinite(const CppAD::AD<Scalar>& value) {
109 return std::isfinite(CppAD::Value(value));
110 }
111
112 namespace CppAD {
113 template <class Scalar>
114 bool isfinite(const CppAD::AD<CppAD::cg::CG<Scalar>>& x) {
115 return std::isfinite(static_cast<Scalar>(CppAD::Value(x).getValue()));
116 }
117 } // namespace CppAD
118
119 namespace Eigen {
120
121 // Overload for Eigen::pow with CppAD-compatible types
122 template <typename Derived>
123 auto pow(const Eigen::ArrayBase<Derived>& base, double exponent) {
124 return base.unaryExpr([exponent](const typename Derived::Scalar& x) {
125 return CppAD::pow(x, typename Derived::Scalar(exponent));
126 });
127 }
128
129 // Overload for Eigen::sqrt with CppAD-compatible types
130 template <typename Derived>
131 typename std::enable_if<std::is_base_of<CppAD::cg::CG<typename Derived::Scalar>,
132 typename Derived::Scalar>::value,
133 Eigen::Array<typename Derived::Scalar, Eigen::Dynamic,
134 Eigen::Dynamic>>::type
135 sqrt(const Eigen::ArrayBase<Derived>& base) {
136 return base.unaryExpr(
137 [](const typename Derived::Scalar& x) { return CppAD::sqrt(x); });
138 }
139
140 } // namespace Eigen
141 #endif
142
143 #endif // CROCODDYL_CORE_UTILS_MATH_HPP_
144