If <composite flatinertia="true">, set mjCBody.explicit_inertial so that an <inertial> element is added in mj_saveLastXML. Bug found by the WriteReadCompare test.

PiperOrigin-RevId: 450702639
Change-Id: I978dfb6713d68ce8327f2e8eea1fc31f43a38b5e

src/user/user_composite.cc

@@ -764,6 +764,8 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body,
     body->inertia[0] = body->mass*(size[1]*size[1]+size[2]*size[2])/3;
     body->inertia[1] = body->mass*(size[0]*size[0]+size[2]*size[2])/3;
     body->inertia[2] = body->mass*(size[0]*size[0]+size[1]*size[1])/3;
+
+    body->MakeInertialExplicit();
   }
 
   // add site

src/user/user_objects.cc

@@ -644,6 +644,10 @@ void mjCBody::MakeLocal(double* _locpos, double* _locquat,
   }
 }
 
+// set explicit_inertial to true
+void mjCBody::MakeInertialExplicit() {
+  explicit_inertial = true;
+}
 
 
 // compiler

src/user/user_objects.h

@@ -160,6 +160,9 @@ class mjCBody : public mjCBase {
   // setup child local frame, take into account change
   void MakeLocal(double* locpos, double* locquat, const double* pos, const double* quat);
 
+  // set explicit_inertial to true
+  void MakeInertialExplicit();
+
   // variables set by user or 'Compile'
   bool mocap;                     // is this a mocap body
   double pos[3];                  // frame position
@@ -188,7 +191,7 @@ class mjCBody : public mjCBase {
   int weldid;                     // top index of body we are welded to
   int dofnum;                     // number of motion dofs for body
   int mocapid;                    // mocap id, -1: not mocap
-  bool explicit_inertial;         // whether inertial clause was explicitly stated
+  bool explicit_inertial;         // whether to save the body with an explicit inertial clause
 
   int lastdof;                    // id of last dof (used by compiler)
 

test/xml/xml_native_writer_test.cc

@@ -332,17 +332,17 @@ static constexpr int kFieldSize = 500;
 // The maximum spacing between a normalised floating point number x and an
 // adjacent normalised number is 2 epsilon |x|; a factor 10 is added accounting
 // for losses during non-idempotent operations such as vector normalizations.
-mjtNum Compare(mjtNum val1, mjtNum val2) {
+template<typename T = mjtNum> T Compare(T val1, T val2) {
-  mjtNum error;
+  T error;
   if (mju_abs(val1) <= 1 || mju_abs(val2) <= 1) {
       // Asbolute precision for small numbers
       error = mju_abs(val1-val2);
   } else {
     // Relative precision for larger numbers
-    mjtNum magnitude = mju_max(mju_abs(val1), mju_abs(val2));
+    T magnitude = mju_max(mju_abs(val1), mju_abs(val2));
     error = mju_abs(val1/magnitude - val2/magnitude) / magnitude;
   }
-  return error < 2*10*std::numeric_limits<double>::epsilon() ? 0 : error;
+  return error < 2*10*std::numeric_limits<T>::epsilon() ? 0 : error;
 }
 
 mjtNum CompareModel(const mjModel* m1, const mjModel* m2, char (&field)[kFieldSize]) {
@@ -388,7 +388,7 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, char (&field)[kFieldSi
 TEST_F(XMLWriterTest, WriteReadCompare) {
   FullFloatPrecision increase_precision;
   // Loop over all xml files in data
-  std::vector<std::string> paths = {GetModelPath("humanoid"), GetModelPath("humanoid100")};
+  std::vector<std::string> paths = {GetModelPath("humanoid"), GetModelPath("flag")};
   std::string ext(".xml");
   for (auto const& path : paths) {
     for (auto &p : std::filesystem::recursive_directory_iterator(path)) {