Add template specializations for several Eigen::numext functions (Rob…

…otLocomotion#15065)

* Add template specializations for several Eigen::numext functions

Specifically, specializations are added for isfinite, isinf, and isnan.
The specializations just forward to internal functions. The reason the
specializations are necessary is that the base templates rely on an
implicit conversion to bool but the internal functions return Formula
which only has an explicit conversion to bool.

common/symbolic_formula.h

@@ -1329,10 +1329,10 @@ struct scalar_cmp_op<drake::symbolic::Variable, drake::symbolic::Variable,
 
 }  // namespace internal
 
+namespace numext {
 // not_equal_strict was only added in Eigen 3.3.5. Since the current minimum
 // Eigen version used by drake is 3.3.4, a version check is needed.
 #if EIGEN_VERSION_AT_LEAST(3, 3, 5)
-namespace numext {
 /// Provides specialization for not_equal_strict with Expression.
 /// As of Eigen 3.4.0, this is called at least as part of triangular vector
 /// solve (though it could also potentially come up elsewhere). The default
@@ -1344,7 +1344,28 @@ EIGEN_STRONG_INLINE bool not_equal_strict(
     const drake::symbolic::Expression& y) {
   return static_cast<bool>(x != y);
 }
-}  // namespace numext
 #endif
+
+// Provides specialization of Eigen::numext::isfinite, numext::isnan, and
+// numext::isinf for Expression. The default template relies on an implicit
+// conversion to bool but our bool operator is explicit, so we need to
+// specialize.
+// As of Eigen 3.4.0, this is called at least as part of JacobiSVD (though
+// it could also potentially come up elsewhere).
+template <>
+EIGEN_STRONG_INLINE bool isfinite(const drake::symbolic::Expression& e) {
+  return static_cast<bool>(drake::symbolic::isfinite(e));
+}
+
+template <>
+EIGEN_STRONG_INLINE bool isinf(const drake::symbolic::Expression& e) {
+  return static_cast<bool>(drake::symbolic::isinf(e));
+}
+
+template <>
+EIGEN_STRONG_INLINE bool isnan(const drake::symbolic::Expression& e) {
+  return static_cast<bool>(drake::symbolic::isnan(e));
+}
+}  // namespace numext
 }  // namespace Eigen
 #endif  // !defined(DRAKE_DOXYGEN_CXX)

common/test/symbolic_expression_test.cc

@@ -894,6 +894,26 @@ TEST_F(SymbolicExpressionTest, EigenNotEqualStrict) {
 }
 
 #endif
+
+// Confirm the other Eigen::numext specializations:
+//  - isfinite
+//  - isnan
+//  - isinf
+// They all trivially forward to our own functions.
+TEST_F(SymbolicExpressionTest, EigenNumext) {
+  // isnan is only valid for non-NaN Expressions. Trying to evaluate
+  // a NaN expression will throw an exception. So we can't check that.
+  EXPECT_FALSE(Eigen::numext::isnan(one_));
+
+  const Expression num_infinity = std::numeric_limits<Expression>::infinity();
+
+  EXPECT_FALSE(Eigen::numext::isinf(one_));
+  EXPECT_TRUE(Eigen::numext::isinf(num_infinity));
+
+  EXPECT_TRUE(Eigen::numext::isfinite(one_));
+  EXPECT_FALSE(Eigen::numext::isfinite(num_infinity));
+}
+
 TEST_F(SymbolicExpressionTest, UnaryMinus) {
   EXPECT_PRED2(ExprEqual, -Expression(var_x_), -var_x_);
   EXPECT_PRED2(ExprNotEqual, c3_, -c3_);