Automated g4 rollback of changelist 604607692.

*** Reason for rollback ***

breakage resulting in "inertia must have positive eigenvalues" when parsing URDF's

*** Original change description ***

Move fullinertia from mjCAlternative to mjCBody.

***

PiperOrigin-RevId: 604677400
Change-Id: Iab8263b5b0aee310bd75404e314f3b707ce3300e

src/user/user_api.h

@@ -30,6 +30,7 @@ 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, model->degree, model->euler);
+  const char* alterr = alt.Set(quat, NULL, model->degree, model->euler);
   if (alterr) {
     return comperr(error, alterr, error_sz);
   }

src/user/user_model.cc

@@ -2551,8 +2551,9 @@ void mjCModel::FuseStatic(void) {
         }
 
         // compute principal axes of inertia
-        mjuu_copyvec(par->fullinertia, toti, 6);
+        mjCAlternative alt;
-        const char* err1 = par->FullInertia(par->iquat, par->inertia);
+        mjuu_copyvec(alt.fullinertia, toti, 6);
+        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

@@ -134,13 +134,14 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos
 
 // constructor
 mjCAlternative::mjCAlternative() {
-  axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = mjNAN;
+  axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = fullinertia[0] = mjNAN;
 }
 
 
 // compute frame orientation given alternative specifications
 // used for geom, site, body and camera frames
-const char* mjCAlternative::Set(double* quat, bool degree, const char* sequence) {
+const char* mjCAlternative::Set(double* quat, double* inertia,
+                                bool degree, const char* sequence) {
   // set quat using axisangle
   if (mjuu_defined(axisangle[0])) {
     // convert to radians if necessary, normalize axis
@@ -194,6 +195,32 @@ const char* mjCAlternative::Set(double* quat, bool degree, const char* sequence)
     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])) {
@@ -553,7 +580,6 @@ 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);
@@ -889,48 +915,17 @@ void mjCBody::GeomFrame(void) {
     }
 
     // compute principal axes of inertia
-    mjuu_copyvec(fullinertia, toti, 6);
+    mjCAlternative alt;
-    const char* errq = FullInertia(iquat, inertia);
+    mjuu_copyvec(alt.fullinertia, toti, 6);
-    if (errq) {
+    const char* err1 = alt.Set(iquat, inertia, model->degree, model->euler);
-      throw mjCError(this, "error '%s' in alternative for principal axes", errq);
+    if (err1) {
+      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;
@@ -962,13 +957,13 @@ void mjCBody::Compile(void) {
   }
 
   // check and process orientation alternatives for body
-  const char* err = alt.Set(quat, model->degree, model->euler);
+  const char* err = alt.Set(quat, inertia, 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 = FullInertia(iquat, inertia);
+  const char* ierr = ialt.Set(iquat, inertia, model->degree, model->euler);
   if (ierr) {
     throw mjCError(this, "error '%s' in inertia alternative", ierr);
   }
@@ -1145,7 +1140,7 @@ void mjCFrame::Compile() {
     return;
   }
 
-  const char* err = alt.Set(quat, model->degree, model->euler);
+  const char* err = alt.Set(quat, 0, model->degree, model->euler);
   if (err) {
     throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
   }
@@ -1828,7 +1823,7 @@ void mjCGeom::Compile(void) {
 
   // not 'fromto': try alternative
   else {
-    const char* err = alt.Set(quat, model->degree, model->euler);
+    const char* err = alt.Set(quat, inertia, model->degree, model->euler);
     if (err) {
       throw mjCError(this, "orientation specification error '%s' in geom %d", err, id);
     }
@@ -1952,7 +1947,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] = mjNAN;
+      spec.alt.euler[0] = spec.alt.fullinertia[0] = mjNAN;
 
   // clear internal variables
   body = 0;
@@ -1992,6 +1987,7 @@ 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);
 }
 
 
@@ -2063,7 +2059,7 @@ void mjCSite::Compile(void) {
 
   // alternative orientation
   else {
-    const char* err = alt_.Set(quat, model->degree, model->euler);
+    const char* err = alt_.Set(quat, 0, model->degree, model->euler);
     if (err) {
       throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
     }
@@ -2129,7 +2125,7 @@ void mjCCamera::Compile(void) {
   userdata.resize(model->nuser_cam);
 
   // process orientation specifications
-  const char* err = alt.Set(quat, model->degree, model->euler);
+  const char* err = alt.Set(quat, 0, 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,                   // set frame quat
+  const char* Set(double* quat, double* inertia,  // set frame quat and diag. inertia
-                  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,10 +244,6 @@ 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
@@ -258,17 +254,16 @@ 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,15 +51,14 @@ void mjXBase::SetModel(mjCModel* _model) {
 
 
 // read alternative orientation specification
-int mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) {
+void 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) +
-  if (read > 1) {
+             (ReadAttr(elem, "fullinertia", 6, alt.fullinertia, text) ? 1 : 0);
-    throw mjXError(elem, "multiple orientation specifiers are not allowed");
+  if (read > 1)
-  }
+    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 int ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
+  static void ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt);
 
  protected:
   mjCModel* model;                    // internally-allocated mjCModel object

src/xml/xml_native_reader.cc

@@ -3021,11 +3021,7 @@ 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);
-      bool alt = ReadAlternative(elem, pbody->ialt);
+      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,17 +242,19 @@ void mjXURDF::Body(XMLElement* body_elem) {
 
     // inertia
     temp = FindSubElem(elem, "inertia", true);
-    ReadAttr(temp, "ixx", 1, pbody->fullinertia+0, text, true);
+    mjCAlternative alt;
-    ReadAttr(temp, "iyy", 1, pbody->fullinertia+1, text, true);
+    ReadAttr(temp, "ixx", 1, alt.fullinertia+0, text, true);
-    ReadAttr(temp, "izz", 1, pbody->fullinertia+2, text, true);
+    ReadAttr(temp, "iyy", 1, alt.fullinertia+1, text, true);
-    ReadAttr(temp, "ixy", 1, pbody->fullinertia+3, text, true);
+    ReadAttr(temp, "izz", 1, alt.fullinertia+2, text, true);
-    ReadAttr(temp, "ixz", 1, pbody->fullinertia+4, text, true);
+    ReadAttr(temp, "ixy", 1, alt.fullinertia+3, text, true);
-    ReadAttr(temp, "iyz", 1, pbody->fullinertia+5, text, true);
+    ReadAttr(temp, "ixz", 1, alt.fullinertia+4, text, true);
+    ReadAttr(temp, "iyz", 1, alt.fullinertia+5, text, true);
 
     // process inertia
     //  lquat = rotation from specified to default (joint/body) inertial frame
     double lquat[4], tmpquat[4];
-    const char* altres = pbody->FullInertia(lquat, pbody->inertia);
+    const char* altres =
+      alt.Set(lquat, pbody->inertia, model->degree, model->euler);
 
     // inertia are sometimes 0 in URDF files: ignore error in altres, fix later
     (void) altres;
@@ -604,7 +606,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, "XYZ");
+      alt.Set(quat, 0, 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 are not allowed"));
+            HasSubstr("multiple orientation specifiers for the same field"));
       }
     }
   }
@@ -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 are not allowed"));
+              HasSubstr("multiple orientation specifiers for the same field"));
 }
 
 TEST_F(XMLReaderTest, ReadShellParameter) {