From e0b6ba4a131c71c8582b5ab75a8c24ab16a2279c Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Thu, 16 Jun 2022 03:08:56 -0700
Subject: [PATCH] Raise warning if there are NaNs in the XML.

For Windows, throw an error if NaNs are present.

PiperOrigin-RevId: 455340787
Change-Id: Ibd95e2335ab15b706ee3328816983e5b57b2315e
---
 src/xml/xml_util.cc                |  5 +++++
 test/xml/xml_native_reader_test.cc | 35 ++++++++++++++++++++++++++++--
 2 files changed, 38 insertions(+), 2 deletions(-)

diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc
index 5d4026fe..6372c85b 100644
--- a/src/xml/xml_util.cc
+++ b/src/xml/xml_util.cc
@@ -26,6 +26,7 @@
 #include <vector>
 
 #include "cc/array_safety.h"
+#include "engine/engine_util_errmem.h"
 #include "xml/xml_util.h"
 #include "xml/xml_numeric_format.h"
 
@@ -567,6 +568,10 @@ int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len,
     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])) {
+        mju_warning("XML contains a 'NaN'. Please check it carefully.");
+      }
     }
   }
   strm >> std::ws;
diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc
index 11edc1ac..22902e4d 100644
--- a/test/xml/xml_native_reader_test.cc
+++ b/test/xml/xml_native_reader_test.cc
@@ -22,13 +22,17 @@
 #include <gtest/gtest.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mujoco.h>
+#include "src/engine/engine_array_safety.h"
+#include "src/engine/engine_util_errmem.h"
 #include "test/fixture.h"
 
 namespace mujoco {
 namespace {
 
+using ::std::string;
 using ::testing::HasSubstr;
 using ::testing::IsNull;
+using ::testing::NotNull;
 
 using UserDataTest = MujocoTest;
 
@@ -128,6 +132,33 @@ TEST_F(UserDataTest, InvalidNUserSensor) {
   EXPECT_THAT(error.data(), HasSubstr("nuser_sensor"));
 }
 
+TEST_F(UserDataTest, RaiseNanWarning) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1" axisangle="1.0 0.0 0.0 nan"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  static char warning[1024];
+  warning[0] = '\0';
+  mju_user_warning = [](const char* msg) {
+    mjSTRNCPY(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
+}
+
 TEST_F(UserDataTest, InvalidArrayElement) {
   static constexpr char xml[] = R"(
   <mujoco>
@@ -188,7 +219,7 @@ TEST_F(UserDataTest, AllowsSpaces) {
   )";
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-  ASSERT_THAT(model, Not(IsNull()));
+  ASSERT_THAT(model, NotNull());
   mj_deleteModel(model);
 }
 
@@ -333,7 +364,7 @@ TEST_F(ActuatorTest, ReadsByte) {
   )";
   std::array<char, 1024> error;
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-  ASSERT_THAT(model, Not(IsNull()));
+  ASSERT_THAT(model, NotNull());
   EXPECT_EQ(*(model->actuator_actlimited), (mjtByte)(1 & 0xFF));
   mj_deleteModel(model);
 }