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