Don't allow quat="0 0 0 0" in XML.

PiperOrigin-RevId: 557535436
Change-Id: Ie71246da5fff67a7cc37cb1801dd88d4a79b5766

src/xml/xml_native_reader.cc

@@ -1414,7 +1414,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
   ReadAttr(elem, "density", 1, &pgeom->density, text);
   ReadAttr(elem, "fromto", 6, pgeom->fromto, text);
   ReadAttr(elem, "pos", 3, pgeom->pos, text);
-  ReadAttr(elem, "quat", 4, pgeom->quat, text);
+  ReadQuat(elem, "quat", pgeom->quat, text);
   ReadAlternative(elem, pgeom->alt);
 
   // compute inertia using either solid or shell geometry
@@ -1441,7 +1441,7 @@ void mjXReader::OneSite(XMLElement* elem, mjCSite* psite) {
   ReadAttr(elem, "size", 3, psite->size, text, false, false);
   ReadAttrInt(elem, "group", &psite->group);
   ReadAttr(elem, "pos", 3, psite->pos, text);
-  ReadAttr(elem, "quat", 4, psite->quat, text);
+  ReadQuat(elem, "quat", psite->quat, text);
   ReadAttrTxt(elem, "material", psite->material);
   ReadAttr(elem, "rgba", 4, psite->rgba, text);
   ReadAttr(elem, "fromto", 6, psite->fromto, text);
@@ -1468,7 +1468,7 @@ void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) {
     pcam->mode = (mjtCamLight)n;
   }
   ReadAttr(elem, "pos", 3, pcam->pos, text);
-  ReadAttr(elem, "quat", 4, pcam->quat, text);
+  ReadQuat(elem, "quat", pcam->quat, text);
   ReadAlternative(elem, pcam->alt);
   ReadAttr(elem, "fovy", 1, &pcam->fovy, text);
   ReadAttr(elem, "ipd", 1, &pcam->ipd, text);
@@ -2678,7 +2678,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
       }
       pbody->explicitinertial = true;
       ReadAttr(elem, "pos", 3, pbody->ipos, text, true);
-      ReadAttr(elem, "quat", 4, pbody->iquat, text);
+      ReadQuat(elem, "quat", pbody->iquat, text);
       ReadAttr(elem, "mass", 1, &pbody->mass, text, true);
       ReadAttr(elem, "diaginertia", 3, pbody->inertia, text);
       ReadAlternative(elem, pbody->ialt);
@@ -2778,7 +2778,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
       ReadAttrTxt(elem, "name", pchild->name);
       ReadAttrTxt(elem, "childclass", pchild->classname);
       ReadAttr(elem, "pos", 3, pchild->pos, text);
-      ReadAttr(elem, "quat", 4, pchild->quat, text);
+      ReadQuat(elem, "quat", pchild->quat, text);
       if (MapValue(elem, "mocap", &n, bool_map, 2)) {
         pchild->mocap = (n==1);
       }

src/xml/xml_util.cc

@@ -715,7 +715,19 @@ template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len
 template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len,
                                mjtByte* data, string& text, bool required, bool exact);
 
+// read quaternion attribute
+//  throw error if identically zero
+int mjXUtil::ReadQuat(XMLElement* elem, const char* attr, double* data, string& text,
+                      bool required) {
+  ReadAttr(elem, attr, /*len=*/4, data, text, required, /*exact=*/true);
 
+  // check for 0 quaternion
+  if (data[0] == 0 && data[1] == 0 && data[2] == 0 && data[3] == 0 ) {
+    throw mjXError(elem, "zero quaternion is not allowed");
+  }
+
+  return 4;
+}
 
 // read DOUBLE array into C++ vector, return number read
 int mjXUtil::ReadVector(XMLElement* elem, const char* attr,

src/xml/xml_util.h

@@ -145,6 +145,9 @@ class mjXUtil {
                       T* data, std::string& text,
                       bool required = false, bool exact = true);
 
+  static int ReadQuat(tinyxml2::XMLElement* elem, const char* attr, double* data,
+                      std::string& text, bool required = false);
+
   // deprecated: use ReadAttrVec
   static int ReadVector(tinyxml2::XMLElement* elem, const char* attr,
                         std::vector<double>& vec, std::string& text, bool required = false);

test/xml/xml_native_reader_test.cc

@@ -328,6 +328,22 @@ TEST_F(XMLReaderTest, InvalidArrayLength) {
   EXPECT_THAT(error.data(), HasSubstr("has too much data"));
 }
 
+TEST_F(XMLReaderTest, InvalidQuaternion) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1" quat="0 0 0 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("zero quaternion is not allowed"));
+}
+
 TEST_F(XMLReaderTest, InvalidNumber) {
   static constexpr char xml[] = R"(
   <mujoco>