Improve full inertia user API.

PiperOrigin-RevId: 627399310
Change-Id: Ie2bdc6778e80b2b8127b52c73a3cc4b4755d8e0a

src/user/user_api.cc

@@ -679,11 +679,12 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
 
 
 // compute full inertia
-const char* mjs_setFullInertia(mjsBody* bodyspec, double quat[4], double inertia[3]) {
+const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
-  mjCBody* body = static_cast<mjCBody*>(bodyspec->element);
+  return FullInertia(quat, inertia, fullinertia);
-  return body->FullInertia(quat, inertia);
 }
 
+
+
 // -------------------------- GLOBAL ASSET CACHE -------------------------------
 
 void mj_setCacheSize(mjCache cache, std::size_t size) {

src/user/user_api.h

@@ -961,8 +961,8 @@ MJAPI void mjs_setFrame(mjElement* dest, mjsFrame* frame);
 MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
                                          const mjsOrientation* orientation);
 
-// Compute quat and inertia from fullinertia, return error if any.
+// Compute quat and diag inertia from full inertia matrix, return error if any.
-MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inertia[3]);
+MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
 
 
 //---------------------------------- Initialization  -----------------------------------------------

src/user/user_model.cc

@@ -2861,7 +2861,7 @@ void mjCModel::FuseStatic(void) {
 
         // compute principal axes of inertia
         mjuu_copyvec(par->fullinertia, toti, 6);
-        const char* err1 = par->FullInertia(par->iquat, par->inertia);
+        const char* err1 = FullInertia(par->iquat, par->inertia, par->fullinertia);
         if (err1) {
           throw mjCError(NULL, "error '%s' in fusing static body inertias", err1);
         }

src/user/user_objects.cc

@@ -96,6 +96,42 @@ static bool islimited(int limited, const double range[2]) {
   return false;
 }
 
+// compute frame quat and diagonal inertia from full inertia matrix, return error if any
+const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
+  if (!mjuu_defined(fullinertia[0])) {
+    return nullptr;
+  }
+
+  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);
+
+  // check mimimal eigenvalue
+  if (eigval[2]<mjEPS) {
+    return "inertia must have positive eigenvalues";
+  }
+
+  // copy
+  if (quat) {
+    for (int i=0; i<4; i++) {
+      quat[i] = quattmp[i];
+    }
+  }
+
+  if (inertia) {
+    for (int i=0; i<3; i++) {
+      inertia[i] = eigval[i];
+    }
+  }
+
+  return nullptr;
+}
+
 
 //------------------------- class mjCError implementation ------------------------------------------
 
@@ -1182,7 +1218,7 @@ void mjCBody::GeomFrame(void) {
 
     // compute principal axes of inertia
     mjuu_copyvec(fullinertia, toti, 6);
-    const char* errq = FullInertia(iquat, inertia);
+    const char* errq = FullInertia(iquat, inertia, fullinertia);
     if (errq) {
       throw mjCError(this, "error '%s' in alternative for principal axes", errq);
     }
@@ -1191,42 +1227,6 @@ void mjCBody::GeomFrame(void) {
 
 
 
-// compute full inertia
-const char* mjCBody::FullInertia(double quat[4], double inertia[3]) {
-  if (!mjuu_defined(fullinertia[0])) {
-    return 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);
-
-  // check mimimal eigenvalue
-  if (eigval[2]<mjEPS) {
-    return "inertia must have positive eigenvalues";
-  }
-
-  // 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];
-    }
-  }
-
-  return 0;
-}
-
-
-
 // set explicitinertial to true
 void mjCBody::MakeInertialExplicit() {
   spec.explicitinertial = true;
@@ -1271,7 +1271,7 @@ void mjCBody::Compile(void) {
   }
 
   // check and process orientation alternatives for inertia
-  const char* ierr = FullInertia(iquat, inertia);
+  const char* ierr = FullInertia(iquat, inertia, this->fullinertia);
   if (ierr) {
     throw mjCError(this, "error '%s' in inertia alternative", ierr);
   }

src/user/user_objects.h

@@ -39,9 +39,9 @@ class mjCSite;
 class mjCCamera;
 class mjCLight;
 class mjCHField;
-class mjCFlex;                        // defined in user_mesh
+class mjCFlex;         // defined in user_mesh.h
-class mjCMesh;                        // defined in user_mesh
+class mjCMesh;         // defined in user_mesh.h
-class mjCSkin;                        // defined in user_mesh
+class mjCSkin;         // defined in user_mesh.h
 class mjCTexture;
 class mjCMaterial;
 class mjCPair;
@@ -55,17 +55,16 @@ class mjCNumeric;
 class mjCText;
 class mjCTuple;
 class mjCDef;
-class mjCModel;                     // defined in user_model
+class mjCModel;        // defined in user_model.h
-class mjXWriter;                    // defined in xml_native
+class mjXWriter;       // defined in xml_native.h
-class mjXURDF;                      // defined in xml_urdf
+class mjXURDF;         // defined in xml_urdf.h
 
 
-//------------------------- helper classes and constants -------------------------------------------
+//------------------------- helper constants, classes and functions --------------------------------
 
 // number of positive size parameters for each geom type
 const int mjGEOMINFO[mjNGEOMTYPES] = {3, 0, 1, 2, 3, 2, 3, 0};
 
-
 // error information
 class [[nodiscard]] mjCError {
  public:
@@ -79,14 +78,14 @@ class [[nodiscard]] mjCError {
   bool warning;                   // is this a warning instead of error
 };
 
-
 // alternative specifications of frame orientation
 const char* ResolveOrientation(double* quat,             // set frame quat
                                bool degree,              // angle format: degree/radian
                                const char* sequence,     // euler sequence format: "xyz"
                                const mjsOrientation& orient);
 
-
+// compute frame quat and diagonal inertia from full inertia matrix, return error if any
+const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
 
 //------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
 
@@ -292,10 +291,6 @@ class mjCBody : public mjCBody_, private mjsBody {
   // 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
   mjsBody spec;
 

src/xml/xml_urdf.cc

@@ -271,7 +271,7 @@ 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 = mjs_setFullInertia(pbody, lquat, pbody->inertia);
+    const char* altres = mjs_fullInertia(lquat, pbody->inertia, pbody->fullinertia);
 
     // inertia are sometimes 0 in URDF files: ignore error in altres, fix later
     (void) altres;