diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 1676bbb1..11c362b0 100644 --- a/src/user/user_composite.cc +++ b/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 diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 6a3cfed1..b2d080b5 100644 --- a/src/user/user_objects.cc +++ b/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 diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 76161f56..413ecbc5 100644 --- a/src/user/user_objects.h +++ b/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) diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 7fe2a73a..4f2e95ca 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/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) { - mjtNum error; +template T Compare(T val1, T val2) { + 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::epsilon() ? 0 : error; + return error < 2*10*std::numeric_limits::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 paths = {GetModelPath("humanoid"), GetModelPath("humanoid100")}; + std::vector paths = {GetModelPath("humanoid"), GetModelPath("flag")}; std::string ext(".xml"); for (auto const& path : paths) { for (auto &p : std::filesystem::recursive_directory_iterator(path)) {