Manually parse inf and nan.

The C++ standard library `std::istringstream` is not guaranteed to parse "inf" and "nan" as valid floating point numbers. In our testing, libc++ does this, but libstdc++ and MSVCRT do not. PiperOrigin-RevId: 455435431 Change-Id: I99c00303b08c2e4e62acfa3ca52a21a3008f4545
2022-06-16 11:57:19 -07:00
parent 69a506e6ba
commit 0e5ce18cff
2 changed files with 76 additions and 14 deletions
@@ -20,6 +20,7 @@
 #include <cstring>
 #include <iostream>
 #include <limits>
+#include <optional>
 #include <sstream>
 #include <string>
 #include <type_traits>
@@ -42,6 +43,32 @@ using tinyxml2::XMLElement;

 namespace mju = ::mujoco::util;

+template <typename T>
+std::optional<T> ParseInfOrNan(const std::string& s) {
+  const char* str = s.c_str();
+  if constexpr (std::is_floating_point_v<T>) {
+    T sign = 1;
+    if (s.size() == 4 && s[0] == '-') {
+      sign = -1;
+      ++str;
+    } else if (s.size() != 3) {
+      return std::nullopt;
+    }
+    if (std::numeric_limits<T>::has_infinity &&
+        (str[0] == 'i' || str[0] == 'I') &&
+        (str[1] == 'n' || str[1] == 'N') &&
+        (str[2] == 'f' || str[2] == 'F')) {
+      return sign * std::numeric_limits<T>::infinity();
+    } else if (std::numeric_limits<T>::has_quiet_NaN &&
+               (str[0] == 'n' || str[0] == 'N') &&
+               (str[1] == 'a' || str[1] == 'A') &&
+               (str[2] == 'n' || str[2] == 'N')) {
+      return sign * std::numeric_limits<T>::quiet_NaN();
+    }
+  }
+  return std::nullopt;
+}
+
 }  // namespace


@@ -565,14 +592,22 @@ int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len,
  while (!strm.eof() && i < len) {
    strm >> token;
    istringstream token_strm(token);
-    token_strm >> data[i++];
+    token_strm >> data[i];
    if (token_strm.fail() || !token_strm.eof()) {
-      throw mjXError(elem, "problem reading attribute '%s'", attr);
-    } else if constexpr (std::is_floating_point_v<T>) {
-      if (std::isnan(data[i-1])) {
+      // C++ standard libraries do not always parse inf and nan as valid floating point values.
+      std::optional<T> maybe_result = ParseInfOrNan<T>(token);
+      if (maybe_result.has_value()) {
+        data[i] = *maybe_result;
+      } else {
+        throw mjXError(elem, "problem reading attribute '%s'", attr);
+      }
+    }
+    if constexpr (std::is_floating_point_v<T>) {
+      if (std::isnan(data[i])) {
        mju_warning("XML contains a 'NaN'. Please check it carefully.");
      }
    }
+    ++i;
  }
  strm >> std::ws;

@@ -16,6 +16,7 @@

 #include <array>
 #include <cstddef>
+#include <limits>
 #include <string>

 #include <gmock/gmock.h>
@@ -32,6 +33,7 @@ namespace {
 using ::std::string;
 using ::testing::Eq;
 using ::testing::HasSubstr;
+using ::testing::IsNan;
 using ::testing::IsNull;
 using ::testing::NotNull;

@@ -133,12 +135,36 @@ TEST_F(UserDataTest, InvalidNUserSensor) {
  EXPECT_THAT(error.data(), HasSubstr("nuser_sensor"));
 }

-TEST_F(UserDataTest, RaiseNanWarning) {
+TEST_F(UserDataTest, CanParseInf) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body>
-        <geom size="1" axisangle="1.0 0.0 0.0 nan"/>
+        <geom size="1" pos="5e-1 -INF iNf"/>
+        <geom size="1" pos="inF -inf Inf"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  const double inf = std::numeric_limits<double>::infinity();
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, NotNull());
+  EXPECT_EQ(model->geom_pos[0], 0.5);
+  EXPECT_EQ(model->geom_pos[1], -inf);
+  EXPECT_THAT(model->geom_pos[2], inf);
+  EXPECT_EQ(model->geom_pos[3], inf);
+  EXPECT_EQ(model->geom_pos[4], -inf);
+  EXPECT_EQ(model->geom_pos[5], inf);
+  mj_deleteModel(model);
+}
+
+TEST_F(UserDataTest, CanParseNanAndRaisesWarning) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1" pos="nan NaN NAN"/>
+        <geom size="1" pos="1.0 0.0 nAn"/>
      </body>
    </worldbody>
  </mujoco>
@@ -150,14 +176,15 @@ TEST_F(UserDataTest, RaiseNanWarning) {
    util::strcpy_arr(warning, msg);
  };
  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-#if defined(_WIN32) || defined(__CYGWIN__)
-    ASSERT_THAT(model, IsNull());
-    EXPECT_THAT(error.data(), HasSubstr("problem reading attribute 'axisangle'"));
-#else
-    ASSERT_THAT(model, NotNull());
-    EXPECT_THAT(warning, HasSubstr("XML contains a 'NaN'"));
-    mj_deleteModel(model);
-#endif
+  ASSERT_THAT(model, NotNull());
+  EXPECT_THAT(warning, HasSubstr("XML contains a 'NaN'"));
+  EXPECT_THAT(model->geom_pos[0], IsNan());
+  EXPECT_THAT(model->geom_pos[1], IsNan());
+  EXPECT_THAT(model->geom_pos[2], IsNan());
+  EXPECT_EQ(model->geom_pos[3], 1);
+  EXPECT_EQ(model->geom_pos[4], 0);
+  EXPECT_THAT(model->geom_pos[5], IsNan());
+  mj_deleteModel(model);
 }

 TEST_F(UserDataTest, InvalidArrayElement) {