From e4fada19633fa95933948a8cb698ecae3e2a2e47 Mon Sep 17 00:00:00 2001
From: Matthew Bennice <mbennice@google.com>
Date: Wed, 7 Feb 2024 11:36:51 -0800
Subject: [PATCH] Correct use of uninitialized value and clean up FullInertia
 function

PiperOrigin-RevId: 605049375
Change-Id: I7ffb6bd56b08111036e8490bf767fb9393499f13
---
 src/user/user_objects.cc | 44 ++++++++++++++++++++++------------------
 src/xml/xml_urdf.cc      |  3 ++-
 2 files changed, 26 insertions(+), 21 deletions(-)

diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
index 9f659a9c..4eb31b63 100644
--- a/src/user/user_objects.cc
+++ b/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])) {
-    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]
-    };
+  if (!mjuu_defined(fullinertia[0])) {
+    return 0;
+  }
 
-    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
-    for (int i=0; i<4; i++) {
-      quat[i] = quattmp[i];
-    }
-    if (inertia) {
-      for (int i=0; i<3; i++) {
-        inertia[i] = eigval[i];
-      }
-    }
+  mju_eig3(eigval, eigvec, quattmp, full);
 
-    // check mimimal eigenvalue
-    if (eigval[2]<mjEPS) {
-      return "inertia must have positive eigenvalues";
+  // 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;
 }
 
diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc
index 8e9197be..00240b2f 100644
--- a/src/xml/xml_urdf.cc
+++ b/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