Fix various issues with body inertial parsing and compiling.

- Add errors in body compiler for incorrect mixing of specifiers.
- Correctly process inertia orientation specifiers.

PiperOrigin-RevId: 676843326
Change-Id: I7e7eb22e25a6769754794f98fad5e0dcc0e5946f

src/user/user_objects.cc

@@ -1497,15 +1497,34 @@ void mjCBody::Compile(void) {
   }
 
   // check and process orientation alternatives for body
-  const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt);
-  if (err) {
-    throw mjCError(this, "error '%s' in frame alternative", err);
+  if (alt.type != mjORIENTATION_QUAT) {
+    const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt);
+    if (err) {
+      throw mjCError(this, "error '%s' in frame alternative", err);
+    }
   }
 
-  // check and process orientation alternatives for inertia
-  const char* ierr = mjuu_fullInertia(iquat, inertia, this->fullinertia);
-  if (ierr) {
-    throw mjCError(this, "error '%s' in inertia alternative", ierr);
+  // check orientation alternatives for inertia
+  if (mjuu_defined(fullinertia[0]) && ialt.type != mjORIENTATION_QUAT) {
+    throw mjCError(this, "fullinertia and inertial orientation cannot both be specified");
+  }
+  if (mjuu_defined(fullinertia[0]) && (inertia[0] || inertia[1] || inertia[2])) {
+    throw mjCError(this, "fullinertia and diagonal inertia cannot both be specified");
+  }
+
+  // process orientation alternatives for inertia
+  if (mjuu_defined(fullinertia[0])) {
+    const char* err = mjuu_fullInertia(iquat, inertia, this->fullinertia);
+    if (err) {
+      throw mjCError(this, "error '%s' in fullinertia", err);
+    }
+  }
+
+  if (ialt.type != mjORIENTATION_QUAT) {
+    const char* err = ResolveOrientation(iquat, model->degree, model->eulerseq, ialt);
+    if (err) {
+      throw mjCError(this, "error '%s' in inertia alternative", err);
+    }
   }
 
   // compile all geoms

src/xml/xml_native_reader.cc

@@ -3443,7 +3443,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
       bool alt = ReadAlternative(elem, body->ialt);
       bool full = ReadAttr(elem, "fullinertia", 6, body->fullinertia, text);
       if (alt && full) {
-        throw mjXError(elem, "fullinertia and orientation specifiers cannot be used together");
+        throw mjXError(elem, "fullinertia and inertial orientation cannot both be specified");
       }
     }
 

src/xml/xml_urdf.cc

@@ -274,9 +274,9 @@ void mjXURDF::Body(XMLElement* body_elem) {
     //  lquat = rotation from specified to default (joint/body) inertial frame
     double lquat[4] = {1, 0, 0, 0};
     double tmpquat[4] = {1, 0, 0, 0};
-    const char* altres = mjuu_fullInertia(lquat, pbody->inertia, pbody->fullinertia);
+    const char* altres = mjuu_fullInertia(lquat, nullptr, pbody->fullinertia);
 
-    // inertia are sometimes 0 in URDF files: ignore error in altres, fix later
+    // inertias are sometimes 0 in URDF files: ignore error in altres, fix later
     (void) altres;
 
     // correct for alignment of full inertia matrix

test/user/user_objects_test.cc

@@ -2487,5 +2487,64 @@ TEST_F(UserObjectsTest, BadWeld) {
   mj_deleteSpec(s);
 }
 
+TEST_F(UserObjectsTest, Inertial) {
+  string xml = R"(
+  <mujoco>
+    <compiler angle="radian"/>
+    <worldbody>
+      <body>
+        <inertial mass="1" pos="2 3 4" euler="3 4 5" diaginertia="4 5 6"/>
+      </body>
+      <body>
+        <inertial mass="2" pos="1 2 3" fullinertia="4 3 2 0 0 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024];
+  mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  EXPECT_EQ(m->body_mass[1], 1);
+  EXPECT_THAT(AsVector(m->body_ipos+3, 3), ElementsAre(2, 3, 4));
+  EXPECT_THAT(AsVector(m->body_inertia+3, 3), ElementsAre(4, 5, 6));
+
+  mjtNum quat[4];
+  const mjtNum euler[3] = {3, 4, 5};
+  mju_euler2Quat(quat, euler, "xyz");
+  EXPECT_EQ(AsVector(m->body_iquat+4, 4), AsVector(quat, 4));
+
+  EXPECT_EQ(m->body_mass[2], 2);
+  EXPECT_THAT(AsVector(m->body_ipos+6, 3), ElementsAre(1, 2, 3));
+  EXPECT_THAT(AsVector(m->body_inertia+6, 3), ElementsAre(4, 3, 2));
+  mj_deleteModel(m);
+
+  string bad_xml1 = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <inertial mass="1" pos="0 0 0" diaginertia="4 5 6" fullinertia="4 3 2 0 0 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  m = LoadModelFromString(bad_xml1.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr("fullinertia and diagonal inertia cannot both"));
+
+  string bad_xml2 = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <inertial mass="1" pos="0 0 0" euler="4 5 6" fullinertia="4 3 2 0 0 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  m = LoadModelFromString(bad_xml2.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot"));
+}
+
 }  // namespace
 }  // namespace mujoco

test/xml/xml_native_reader_test.cc

@@ -1640,7 +1640,7 @@ TEST_F(XMLReaderTest, InvalidInertialOrientation) {
   EXPECT_THAT(
       error.data(),
       HasSubstr(
-          "fullinertia and orientation specifiers cannot be used together"));
+          "fullinertia and inertial orientation cannot both be specified"));
 }
 
 TEST_F(XMLReaderTest, ReadShellParameter) {