Correct use of uninitialized value and clean up FullInertia function

PiperOrigin-RevId: 605049375
Change-Id: I7ffb6bd56b08111036e8490bf767fb9393499f13

src/user/user_objects.cc

@@ -921,31 +921,35 @@ void mjCBody::GeomFrame(void) {
 
 // compute full inertia
 const char* mjCBody::FullInertia(double quat[4], double inertia[3]) {
-  if (mjuu_defined(fullinertia[0])) {
+  if (!mjuu_defined(fullinertia[0])) {
-    mjtNum eigval[3], eigvec[9], quattmp[4];
+    return 0;
-    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);
+  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]
+  };
 
-    // copy
+  mju_eig3(eigval, eigvec, quattmp, full);
-    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
+  // check mimimal eigenvalue
-    if (eigval[2]<mjEPS) {
+  if (eigval[2]<mjEPS) {
-      return "inertia must have positive eigenvalues";
+    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;
 }
 

src/xml/xml_urdf.cc

@@ -266,7 +266,8 @@ void mjXURDF::Body(XMLElement* body_elem) {
 
     // process inertia
     //  lquat = rotation from specified to default (joint/body) inertial frame
-    double lquat[4], tmpquat[4];
+    double lquat[4] = {1, 0, 0, 0};
+    double tmpquat[4] = {1, 0, 0, 0};
     const char* altres = mjm_setFullInertia(pbody, lquat, pbody->inertia);
 
     // inertia are sometimes 0 in URDF files: ignore error in altres, fix later