Renamed mjs_fullInertia to mjuu_fullInertia and moved it to user_util.

PiperOrigin-RevId: 644769379
Change-Id: I83511eeb4c0972ce9f46f0e3443d438cccdd4d9f

doc/APIreference/functions.rst

@@ -4341,15 +4341,6 @@ mjs_resolveOrientation
 
 Resolve alternative orientations to quat, return error if any.
 
-.. _mjs_fullInertia:
-
-mjs_fullInertia
-~~~~~~~~~~~~~~~
-
-.. mujoco-include:: mjs_fullInertia
-
-Compute quat and diag inertia from full inertia matrix, return error if any.
-
 .. _ElementInitialization:
 
 Element initialization

doc/includes/references.h

@@ -3559,7 +3559,6 @@ void mjs_setDefault(mjsElement* element, mjsDefault* def);
 void mjs_setFrame(mjsElement* dest, mjsFrame* frame);
 const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
                                    const mjsOrientation* orientation);
-const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
 void mjs_defaultSpec(mjSpec* spec);
 void mjs_defaultOrientation(mjsOrientation* orient);
 void mjs_defaultBody(mjsBody* body);

include/mujoco/mujoco.h

@@ -1612,9 +1612,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame);
 MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
                                          const mjsOrientation* orientation);
 
-// Compute quat and diag inertia from full inertia matrix, return error if any.
-MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
-
 
 //---------------------------------- Element initialization  ---------------------------------------
 

introspect/functions.py

@@ -10216,37 +10216,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Resolve alternative orientations to quat, return error if any.',
      )),
-    ('mjs_fullInertia',
-     FunctionDecl(
-         name='mjs_fullInertia',
-         return_type=PointerType(
-             inner_type=ValueType(name='char', is_const=True),
-         ),
-         parameters=(
-             FunctionParameterDecl(
-                 name='quat',
-                 type=ArrayType(
-                     inner_type=ValueType(name='double'),
-                     extents=(4,),
-                 ),
-             ),
-             FunctionParameterDecl(
-                 name='inertia',
-                 type=ArrayType(
-                     inner_type=ValueType(name='double'),
-                     extents=(3,),
-                 ),
-             ),
-             FunctionParameterDecl(
-                 name='fullinertia',
-                 type=ArrayType(
-                     inner_type=ValueType(name='double', is_const=True),
-                     extents=(6,),
-                 ),
-             ),
-         ),
-         doc='Compute quat and diag inertia from full inertia matrix, return error if any.',  # pylint: disable=line-too-long
-     )),
     ('mjs_defaultSpec',
      FunctionDecl(
          name='mjs_defaultSpec',

src/user/user_api.cc

@@ -696,13 +696,6 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) {
 
 
 
-// compute full inertia
-const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
-  return FullInertia(quat, inertia, fullinertia);
-}
-
-
-
 // -------------------------- GLOBAL ASSET CACHE -------------------------------
 
 void mj_setCacheSize(mjCache cache, std::size_t size) {

src/user/user_api.h

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

src/user/user_model.cc

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

@@ -91,42 +91,6 @@ 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;
-  }
-
-  double eigval[3], eigvec[9], quattmp[4];
-  double full[9] = {
-    fullinertia[0], fullinertia[3], fullinertia[4],
-    fullinertia[3], fullinertia[1], fullinertia[5],
-    fullinertia[4], fullinertia[5], fullinertia[2]
-  };
-
-  mjuu_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;
-}
-
 
 
 // fetches cached image from PNG asset, returns nullopt if not available
@@ -1360,7 +1324,7 @@ void mjCBody::GeomFrame(void) {
 
     // compute principal axes of inertia
     mjuu_copyvec(fullinertia, toti, 6);
-    const char* errq = FullInertia(iquat, inertia, fullinertia);
+    const char* errq = mjuu_fullInertia(iquat, inertia, fullinertia);
     if (errq) {
       throw mjCError(this, "error '%s' in alternative for principal axes", errq);
     }
@@ -1429,7 +1393,7 @@ void mjCBody::Compile(void) {
   }
 
   // check and process orientation alternatives for inertia
-  const char* ierr = FullInertia(iquat, inertia, this->fullinertia);
+  const char* ierr = mjuu_fullInertia(iquat, inertia, this->fullinertia);
   if (ierr) {
     throw mjCError(this, "error '%s' in inertia alternative", ierr);
   }

src/user/user_objects.h

@@ -86,8 +86,6 @@ const char* ResolveOrientation(double* quat,             // set frame quat
                                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 ---------------------------------------
 

src/user/user_util.cc

@@ -751,6 +751,39 @@ string mjuu_strippath(string filename) {
 }
 
 
+// compute frame quat and diagonal inertia from full inertia matrix, return error if any
+const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
+  if (!mjuu_defined(fullinertia[0])) {
+    return nullptr;
+  }
+
+  double eigval[3], eigvec[9], quattmp[4];
+  double full[9] = {
+    fullinertia[0], fullinertia[3], fullinertia[4],
+    fullinertia[3], fullinertia[1], fullinertia[5],
+    fullinertia[4], fullinertia[5], fullinertia[2]
+  };
+
+  mjuu_eig3(eigval, eigvec, quattmp, full);
+
+  // check mimimal eigenvalue
+  if (eigval[2]<mjEPS) {
+    return "inertia must have positive eigenvalues";
+  }
+
+  // copy
+  if (quat) {
+    mjuu_copyvec(quat, quattmp, 4);
+  }
+
+  if (inertia) {
+    mjuu_copyvec(inertia, eigval, 3);
+  }
+
+  return nullptr;
+}
+
+
 // strip extension
 string mjuu_stripext(string filename) {
   // find last dot

src/user/user_util.h

@@ -152,6 +152,9 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
 // transform vector by pose
 void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]);
 
+// compute frame quat and diagonal inertia from full inertia matrix, return error if any
+const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]);
+
 // strip path from filename
 std::string mjuu_strippath(std::string filename);
 

src/xml/xml_urdf.cc

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