Move fullinertia from mjCAlternative to mjCBody with correction to handle URDF with 0 inertias.

PiperOrigin-RevId: 604812870
Change-Id: I018254d1362a654a4f99bf5e621d5abc7129a0c6

src/user/user_api.h

@@ -30,7 +30,6 @@ typedef struct _mjmOrientation {
   double xyaxes[6];               // x and y axes
   double zaxis[3];                // z axis (use minimal rotation)
   double euler[3];                // euler rotations
-  double fullinertia[6];          // non-axis-aligned inertia matrix
 } mjmOrientation;
 
 typedef struct _mjmSite {

src/user/user_flexcomp.cc

@@ -117,7 +117,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz
   }
 
   // compute orientation
-  const char* alterr = alt.Set(quat, NULL, model->degree, model->euler);
+  const char* alterr = alt.Set(quat, model->degree, model->euler);
   if (alterr) {
     return comperr(error, alterr, error_sz);
   }

src/user/user_model.cc

@@ -20,6 +20,7 @@
 #include <cstdint>
 #include <cstdlib>
 #include <cstring>
+#include <iostream>
 #include <map>
 #include <string>
 #include <string_view>
@@ -2551,9 +2552,8 @@ void mjCModel::FuseStatic(void) {
         }
 
         // compute principal axes of inertia
-        mjCAlternative alt;
+        mjuu_copyvec(par->fullinertia, toti, 6);
-        mjuu_copyvec(alt.fullinertia, toti, 6);
+        const char* err1 = par->FullInertia(par->iquat, par->inertia);
-        const char* err1 = alt.Set(par->iquat, par->inertia, degree, euler);
         if (err1) {
           throw mjCError(NULL, "error '%s' in fusing static body inertias", err1);
         }

src/user/user_objects.cc

@@ -20,6 +20,7 @@
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
+#include <iostream>
 #include <optional>
 #include <sstream>
 #include <string>
@@ -134,14 +135,13 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos
 
 // constructor
 mjCAlternative::mjCAlternative() {
-  axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = fullinertia[0] = mjNAN;
+  axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = mjNAN;
 }
 
 
 // compute frame orientation given alternative specifications
 // used for geom, site, body and camera frames
-const char* mjCAlternative::Set(double* quat, double* inertia,
+const char* mjCAlternative::Set(double* quat, bool degree, const char* sequence) {
-                                bool degree, const char* sequence) {
   // set quat using axisangle
   if (mjuu_defined(axisangle[0])) {
     // convert to radians if necessary, normalize axis
@@ -195,32 +195,6 @@ const char* mjCAlternative::Set(double* quat, double* inertia,
     mjuu_z2quat(quat, zaxis);
   }
 
-  // handle fullinertia
-  if (mjuu_defined(fullinertia[0])) {
-    mjtNum eigval[3], eigvec[9], quattmp[4];
-    mjtNum full[9] = {
-      fullinertia[0], fullinertia[3], fullinertia[4],
-      fullinertia[3], fullinertia[1], fullinertia[5],
-      fullinertia[4], fullinertia[5], fullinertia[2]
-    };
-
-    mju_eig3(eigval, eigvec, quattmp, full);
-
-    // copy
-    for (int i=0; i<4; i++) {
-      quat[i] = quattmp[i];
-    }
-    if (inertia) {
-      for (int i=0; i<3; i++) {
-        inertia[i] = eigval[i];
-      }
-    }
-
-    // check mimimal eigenvalue
-    if (eigval[2]<mjEPS) {
-      return "inertia must have positive eigenvalues";
-    }
-  }
 
   // handle euler
   if (mjuu_defined(euler[0])) {
@@ -580,6 +554,7 @@ mjCBody::mjCBody(mjCModel* _model) {
   pos[0] = ipos[0] = mjNAN;
 
   // clear variables
+  fullinertia[0] = mjNAN;
   explicitinertial = false;
   mocap = false;
   mjuu_setvec(quat, 1, 0, 0, 0);
@@ -915,17 +890,48 @@ void mjCBody::GeomFrame(void) {
     }
 
     // compute principal axes of inertia
-    mjCAlternative alt;
+    mjuu_copyvec(fullinertia, toti, 6);
-    mjuu_copyvec(alt.fullinertia, toti, 6);
+    const char* errq = FullInertia(iquat, inertia);
-    const char* err1 = alt.Set(iquat, inertia, model->degree, model->euler);
+    if (errq) {
-    if (err1) {
+      throw mjCError(this, "error '%s' in alternative for principal axes", errq);
-      throw mjCError(this, "error '%s' in alternative for principal axes", err1);
     }
   }
 }
 
 
 
+// compute full inertia
+const char* mjCBody::FullInertia(double quat[4], double inertia[3]) {
+  if (mjuu_defined(fullinertia[0])) {
+    mjtNum eigval[3], eigvec[9], quattmp[4];
+    mjtNum full[9] = {
+      fullinertia[0], fullinertia[3], fullinertia[4],
+      fullinertia[3], fullinertia[1], fullinertia[5],
+      fullinertia[4], fullinertia[5], fullinertia[2]
+    };
+
+    mju_eig3(eigval, eigvec, quattmp, full);
+
+    // copy
+    for (int i=0; i<4; i++) {
+      quat[i] = quattmp[i];
+    }
+    if (inertia) {
+      for (int i=0; i<3; i++) {
+        inertia[i] = eigval[i];
+      }
+    }
+
+    // check mimimal eigenvalue
+    if (eigval[2]<mjEPS) {
+      return "inertia must have positive eigenvalues";
+    }
+  }
+  return 0;
+}
+
+
+
 // set explicitinertial to true
 void mjCBody::MakeInertialExplicit() {
   explicitinertial = true;
@@ -957,13 +963,13 @@ void mjCBody::Compile(void) {
   }
 
   // check and process orientation alternatives for body
-  const char* err = alt.Set(quat, inertia, model->degree, model->euler);
+  const char* err = alt.Set(quat, model->degree, model->euler);
   if (err) {
     throw mjCError(this, "error '%s' in frame alternative", err);
   }
 
   // check and process orientation alternatives for inertia
-  const char* ierr = ialt.Set(iquat, inertia, model->degree, model->euler);
+  const char* ierr = FullInertia(iquat, inertia);
   if (ierr) {
     throw mjCError(this, "error '%s' in inertia alternative", ierr);
   }
@@ -1140,7 +1146,7 @@ void mjCFrame::Compile() {
     return;
   }
 
-  const char* err = alt.Set(quat, 0, model->degree, model->euler);
+  const char* err = alt.Set(quat, model->degree, model->euler);
   if (err) {
     throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
   }
@@ -1823,7 +1829,7 @@ void mjCGeom::Compile(void) {
 
   // not 'fromto': try alternative
   else {
-    const char* err = alt.Set(quat, inertia, model->degree, model->euler);
+    const char* err = alt.Set(quat, model->degree, model->euler);
     if (err) {
       throw mjCError(this, "orientation specification error '%s' in geom %d", err, id);
     }
@@ -1947,7 +1953,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
   spec.fromto[0] = mjNAN;
   spec_userdata_.clear();
   spec.alt.axisangle[0] = spec.alt.xyaxes[0] = spec.alt.zaxis[0] =
-      spec.alt.euler[0] = spec.alt.fullinertia[0] = mjNAN;
+      spec.alt.euler[0] = mjNAN;
 
   // clear internal variables
   body = 0;
@@ -1987,7 +1993,6 @@ void mjCSite::CopyFromSpec() {
   mju_copy(alt_.xyaxes, alt.xyaxes, 6);
   mju_copy3(alt_.zaxis, alt.zaxis);
   mju_copy3(alt_.euler, alt.euler);
-  mju_copy(alt_.fullinertia, alt.fullinertia, 6);
 }
 
 
@@ -2059,7 +2064,7 @@ void mjCSite::Compile(void) {
 
   // alternative orientation
   else {
-    const char* err = alt_.Set(quat, 0, model->degree, model->euler);
+    const char* err = alt_.Set(quat, model->degree, model->euler);
     if (err) {
       throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
     }
@@ -2125,7 +2130,7 @@ void mjCCamera::Compile(void) {
   userdata.resize(model->nuser_cam);
 
   // process orientation specifications
-  const char* err = alt.Set(quat, 0, model->degree, model->euler);
+  const char* err = alt.Set(quat, model->degree, model->euler);
   if (err) {
     throw mjCError(this, "orientation specification error '%s' in camera %d", err, id);
   }

src/user/user_objects.h

@@ -111,9 +111,9 @@ class [[nodiscard]] mjCError {
 class mjCAlternative : public mjmOrientation {
  public:
   mjCAlternative();                               // constuctor
-  const char* Set(double* quat, double* inertia,  // set frame quat and diag. inertia
+  const char* Set(double* quat,                   // set frame quat
-                  bool degree,                    //  angle format: degree/radian
+                  bool degree,                    // angle format: degree/radian
-                  const char* sequence);          //  euler sequence format: "xyz"
+                  const char* sequence);          // euler sequence format: "xyz"
 };
 
 
@@ -244,6 +244,10 @@ class mjCBody : public mjCBase {
   // set explicitinertial to true
   void MakeInertialExplicit();
 
+  // compute quat and diag inertia from fullinertia
+  // return nullptr on success, error string on failure
+  const char* FullInertia(double quat[4], double inertia[3]);
+
   // variables set by user or 'Compile'
   bool mocap;                     // is this a mocap body
   double pos[3];                  // frame position
@@ -254,16 +258,17 @@ class mjCBody : public mjCBase {
   double inertia[3];              // diagonal inertia (in i-frame)
   double gravcomp;                // gravity compensation
   std::vector<double> userdata;   // user data
+  double fullinertia[6];          // non-axis-aligned inertia matrix
   mjCAlternative alt;             // alternative orientation specification
   mjCAlternative ialt;            // alternative for inertial frame
 
   // variables computed by 'Compile' and 'AddXXX'
  private:
-  mjCBody(mjCModel*);                 // constructor
+  mjCBody(mjCModel*);             // constructor
-  ~mjCBody();                         // destructor
+  ~mjCBody();                     // destructor
   void Compile(void);                 // compiler
 
-  void GeomFrame(void);               // get inertial info from geoms
+  void GeomFrame(void);           // get inertial info from geoms
 
   int parentid;                   // parent index in global array
   int weldid;                     // top index of body we are welded to

src/xml/xml_base.cc

@@ -51,14 +51,15 @@ void mjXBase::SetModel(mjCModel* _model) {
 
 
 // read alternative orientation specification
-void mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) {
+int mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) {
   string text;
   int read = (int)(elem->Attribute("quat") != 0) +
              (ReadAttr(elem, "axisangle", 4, alt.axisangle, text) ? 1 : 0) +
              (ReadAttr(elem, "xyaxes", 6, alt.xyaxes, text) ? 1 : 0) +
              (ReadAttr(elem, "zaxis", 3, alt.zaxis, text) ? 1 : 0) +
-             (ReadAttr(elem, "euler", 3, alt.euler, text) ? 1 : 0) +
+             (ReadAttr(elem, "euler", 3, alt.euler, text) ? 1 : 0);
-             (ReadAttr(elem, "fullinertia", 6, alt.fullinertia, text) ? 1 : 0);
+  if (read > 1) {
-  if (read > 1)
+    throw mjXError(elem, "multiple orientation specifiers are not allowed");
-    throw mjXError(elem, "multiple orientation specifiers for the same field are not allowed");
+  }
+  return read;
 }

src/xml/xml_base.h

@@ -92,7 +92,7 @@ class mjXBase : public mjXUtil {
   void SetModel(mjCModel*);
 
   // read alternative orientation specification
-  static void ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
+  static int ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
 
  protected:
   mjCModel* model;                    // internally-allocated mjCModel object

src/xml/xml_native_reader.cc

@@ -3021,7 +3021,11 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) {
       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);
+      bool alt = ReadAlternative(elem, pbody->ialt);
+      bool full = ReadAttr(elem, "fullinertia", 6, pbody->fullinertia, text);
+      if (alt && full) {
+        throw mjXError(elem, "multiple orientation specifiers are not allowed");
+      }
     }
 
     // joint sub-element

src/xml/xml_urdf.cc

@@ -242,19 +242,29 @@ void mjXURDF::Body(XMLElement* body_elem) {
 
     // inertia
     temp = FindSubElem(elem, "inertia", true);
-    mjCAlternative alt;
+    ReadAttr(temp, "ixx", 1, pbody->fullinertia+0, text, true);
-    ReadAttr(temp, "ixx", 1, alt.fullinertia+0, text, true);
+    ReadAttr(temp, "iyy", 1, pbody->fullinertia+1, text, true);
-    ReadAttr(temp, "iyy", 1, alt.fullinertia+1, text, true);
+    ReadAttr(temp, "izz", 1, pbody->fullinertia+2, text, true);
-    ReadAttr(temp, "izz", 1, alt.fullinertia+2, text, true);
+    ReadAttr(temp, "ixy", 1, pbody->fullinertia+3, text, true);
-    ReadAttr(temp, "ixy", 1, alt.fullinertia+3, text, true);
+    ReadAttr(temp, "ixz", 1, pbody->fullinertia+4, text, true);
-    ReadAttr(temp, "ixz", 1, alt.fullinertia+4, text, true);
+    ReadAttr(temp, "iyz", 1, pbody->fullinertia+5, text, true);
-    ReadAttr(temp, "iyz", 1, alt.fullinertia+5, text, true);
+
+    // If the inertias are all 0 in a URDF then it is still undefined.
+    bool inertia_defined = false;
+    for (int i = 0; i < 6; ++i) {
+      if (pbody->fullinertia[i] != 0) {
+        inertia_defined = true;
+        break;
+      }
+    }
+    if (!inertia_defined) {
+      pbody->fullinertia[0] = mjNAN;
+    }
 
     // process inertia
     //  lquat = rotation from specified to default (joint/body) inertial frame
     double lquat[4], tmpquat[4];
-    const char* altres =
+    const char* altres = pbody->FullInertia(lquat, pbody->inertia);
-      alt.Set(lquat, pbody->inertia, model->degree, model->euler);
 
     // inertia are sometimes 0 in URDF files: ignore error in altres, fix later
     (void) altres;
@@ -606,7 +616,7 @@ void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) {
     // orientation
     mjCAlternative alt;
     if (ReadAttr(temp, "rpy", 3, alt.euler, text)) {
-      alt.Set(quat, 0, 0, "XYZ");
+      alt.Set(quat, 0, "XYZ");
     }
   }
 }

test/xml/xml_native_reader_test.cc

@@ -414,7 +414,7 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) {
         ASSERT_THAT(model, IsNull());
         EXPECT_THAT(
             error.data(),
-            HasSubstr("multiple orientation specifiers for the same field"));
+            HasSubstr("multiple orientation specifiers are not allowed"));
       }
     }
   }
@@ -525,7 +525,7 @@ TEST_F(XMLReaderTest, InvalidInertialOrientation) {
   mjModel* model = LoadModelFromString(xml, error.data(), error.size());
   ASSERT_THAT(model, IsNull());
   EXPECT_THAT(error.data(),
-              HasSubstr("multiple orientation specifiers for the same field"));
+              HasSubstr("multiple orientation specifiers are not allowed"));
 }
 
 TEST_F(XMLReaderTest, ReadShellParameter) {