GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/utils/math.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 17 17 100.0%
Branches: 18 36 50.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/Dense>
13 #include <algorithm>
14 #include <cmath>
15 #include <limits>
16 #include <type_traits>
17
18 #ifdef CROCODDYL_WITH_CODEGEN
19 #include <cppad/cg/support/cppadcg_eigen.hpp>
20 #endif
21
22 #ifdef CROCODDYL_WITH_CODEGEN
23 #include <cppad/cg/cg.hpp>
24 #include <cppad/cppad.hpp>
25 #endif
26
27 namespace crocoddyl {
28
29 template <typename Scalar>
30 1882 constexpr Scalar pi() {
31 1882 return static_cast<Scalar>(3.14159265358979323846264338327950288);
32 }
33
34 template <typename Scalar>
35 typename std::enable_if<std::is_floating_point<Scalar>::value, bool>::type
36 78299 isfinite(const Scalar& value) {
37 78299 return std::isfinite(value);
38 }
39
40 template <typename MatrixLike,
41 bool value =
42 (Eigen::NumTraits<typename MatrixLike::Scalar>::IsInteger == 0)>
43 struct pseudoInverseAlgo {
44 typedef typename MatrixLike::Scalar Scalar;
45 typedef typename MatrixLike::RealScalar RealScalar;
46
47 11314 static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
48 const RealScalar& epsilon) {
49 using std::max;
50
2/4
✓ Branch 1 taken 11314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11314 times.
✗ Branch 5 not taken.
 11314 Eigen::JacobiSVD<MatrixLike> svd(a,
51 Eigen::ComputeThinU | Eigen::ComputeThinV);
52 33942 RealScalar tolerance = epsilon *
53 11314 static_cast<Scalar>(max(a.cols(), a.rows())) *
54
4/8
✓ Branch 1 taken 11314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11314 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11314 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11314 times.
✗ Branch 11 not taken.
 11314 svd.singularValues().array().abs()(0);
55 // FIX: Replace select() with a lambda function
56 Eigen::Matrix<typename MatrixLike::Scalar, Eigen::Dynamic, 1>
57
1/2
✓ Branch 1 taken 11314 times.
✗ Branch 2 not taken.
 11314 invSingularValues =
58
2/4
✓ Branch 1 taken 11314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11314 times.
✗ Branch 5 not taken.
 11314 svd.singularValues().unaryExpr([&](const Scalar& x) {
59
2/4
✓ Branch 0 taken 194148 times.
✓ Branch 1 taken 513 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 194661 return (x > tolerance) ? Scalar(1) / x : Scalar(0);
60 });
61
4/8
✓ Branch 1 taken 11314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11314 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11314 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11314 times.
✗ Branch 11 not taken.
 11314 return svd.matrixV() * invSingularValues.asDiagonal() *
62
3/6
✓ Branch 1 taken 11314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11314 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11314 times.
✗ Branch 8 not taken.
 33942 svd.matrixU().adjoint();
63 11314 }
64 };
65
66 template <typename MatrixLike>
67 struct pseudoInverseAlgo<MatrixLike, false> {
68 typedef typename MatrixLike::Scalar Scalar;
69 typedef typename MatrixLike::RealScalar RealScalar;
70
71 static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
72 const RealScalar&) {
73 return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
74 }
75 };
76
77 template <typename MatrixLike>
78 11314 MatrixLike pseudoInverse(
79 const Eigen::MatrixBase<MatrixLike>& a,
80 const typename MatrixLike::RealScalar& epsilon =
81 Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
82 11314 return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
83 }
84
85 } // namespace crocoddyl
86
87 #ifdef CROCODDYL_WITH_CODEGEN
88 template <typename Scalar>
89 bool isfinite(const CppAD::AD<Scalar>& value) {
90 return std::isfinite(CppAD::Value(value));
91 }
92
93 namespace CppAD {
94 template <class Scalar>
95 bool isfinite(const CppAD::AD<CppAD::cg::CG<Scalar>>& x) {
96 return std::isfinite(static_cast<Scalar>(CppAD::Value(x).getValue()));
97 }
98 } // namespace CppAD
99
100 namespace Eigen {
101
102 // Overload for Eigen::pow with CppAD-compatible types
103 template <typename Derived>
104 auto pow(const Eigen::ArrayBase<Derived>& base, double exponent) {
105 return base.unaryExpr([exponent](const typename Derived::Scalar& x) {
106 return CppAD::pow(x, typename Derived::Scalar(exponent));
107 });
108 }
109
110 // Overload for Eigen::sqrt with CppAD-compatible types
111 template <typename Derived>
112 typename std::enable_if<std::is_base_of<CppAD::cg::CG<typename Derived::Scalar>,
113 typename Derived::Scalar>::value,
114 Eigen::Array<typename Derived::Scalar, Eigen::Dynamic,
115 Eigen::Dynamic>>::type
116 sqrt(const Eigen::ArrayBase<Derived>& base) {
117 return base.unaryExpr(
118 [](const typename Derived::Scalar& x) { return CppAD::sqrt(x); });
119 }
120
121 } // namespace Eigen
122 #endif
123
124 #endif // CROCODDYL_CORE_UTILS_MATH_HPP_
125