diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index ad5fc22d..419835d5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -81,6 +81,11 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { face = NULL; graph = NULL; needhull = false; + validorientation = true; + validarea = true; + validvolume = true; + valideigenvalue = true; + validinequality = true; // reset to default if given if (_def) { @@ -223,8 +228,9 @@ void mjCMesh::Compile(const mjVFS* vfs) { if (!useredge.empty()) { std::sort(useredge.begin(), useredge.end()); auto iterator = std::adjacent_find(useredge.begin(), useredge.end()); - if (iterator != useredge.end()) - throw mjCError(this, "faces have inconsistent orientation"); + if (iterator != useredge.end()) { + validorientation = false; + } } // require vertices @@ -248,7 +254,9 @@ void mjCMesh::Compile(const mjVFS* vfs) { } // scale, center, orient, compute mass and inertia - Process(); + if (validorientation) { + Process(); + } } @@ -920,7 +928,7 @@ void mjCMesh::Process() { vert[3*i+1] = (float) p1[1]; vert[3*i+2] = (float) p1[2]; - // nromals + // normals mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]}; mju_rotVecMatT(n1, n0, mat); normal[3*i] = (float) n1[0]; @@ -983,7 +991,8 @@ void mjCMesh::Process() { // require positive area if (area < mjMINVAL) { - throw mjCError(this, "mesh surface area is too small: %s", name.c_str()); + validarea = false; + return; } // finalize centroid of faces @@ -1016,7 +1025,8 @@ void mjCMesh::Process() { // require positive volume if (GetVolumeRef(type) < mjMINVAL) { - throw mjCError(this, "mesh volume is too small: %s", name.c_str()); + validvolume = false; + return; } // finalize CoM, save as mesh center @@ -1085,13 +1095,14 @@ void mjCMesh::Process() { // check eigval - SHOULD NOT OCCUR if (eigval[2]<=0) { - throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str()); + valideigenvalue = false; + return; } if (eigval[0] + eigval[1] < eigval[2] || eigval[0] + eigval[2] < eigval[1] || eigval[1] + eigval[2] < eigval[0]) { - throw mjCError(this, - "eigenvalues of mesh inertia violate A + B >= C condition: %s", name.c_str()); + validinequality = false; + return; } // compute sizes of equivalent inertia box @@ -1140,8 +1151,24 @@ void mjCMesh::Process() { } +// check that the mesh is valid +void mjCMesh::CheckMesh() { + if (!validorientation) + throw mjCError(this, "faces have inconsistent orientation: %s", name.c_str()); + if (!validarea) + throw mjCError(this, "mesh surface area is too small: %s", name.c_str()); + if (!validvolume) + throw mjCError(this, "mesh volume is too small: %s", name.c_str()); + if (!valideigenvalue) + throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str()); + if (!validinequality) + throw mjCError(this, "eigenvalues of mesh inertia violate A + B >= C: %s", name.c_str()); +} + + // compute inertia double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) { + CheckMesh(); if (type==mjSHELL_MESH) { return boxsz_surface; } else { @@ -1151,6 +1178,7 @@ double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) { double& mjCMesh::GetVolumeRef(mjtMeshType type) { + CheckMesh(); if (type) { return surface; } else { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 0a4a296c..d7d49905 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -295,7 +295,7 @@ mjCBody::mjCBody(mjCModel* _model) { pos[0] = ipos[0] = mjNAN; // clear variables - explicit_inertial = false; + explicitinertial = false; mocap = false; mjuu_setvec(quat, 1, 0, 0, 0); mjuu_setvec(iquat, 1, 0, 0, 0); @@ -634,9 +634,9 @@ void mjCBody::MakeLocal(double* _locpos, double* _locquat, } } -// set explicit_inertial to true +// set explicitinertial to true void mjCBody::MakeInertialExplicit() { - explicit_inertial = true; + explicitinertial = true; } @@ -680,6 +680,8 @@ void mjCBody::Compile(void) { // compile all geoms, phase 1 for (i=0; iinferinertia = id>0 && (!explicitinertial || + model->inertiafromgeom==mjINERTIAFROMGEOM_TRUE); geoms[i]->Compile(); } @@ -968,6 +970,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { rgba[3] = 1.0f; userdata.clear(); typeinertia = mjVOLUME_MESH; + inferinertia = true; // clear internal variables mjuu_setvec(quat, 1, 0, 0, 0); @@ -1385,18 +1388,26 @@ void mjCGeom::Compile(void) { } // compute geom mass and inertia - if (mjuu_defined(_mass) && GetVolume()>mjMINVAL) { - mass = _mass; - density = _mass / GetVolume(); - } else { - mass = density * GetVolume(); - } - SetInertia(); + if (inferinertia) { + if (mjuu_defined(_mass)) { + if (_mass==0) { + mass = 0; + density = 0; + } else if (GetVolume()>mjMINVAL) { + mass = _mass; + density = _mass / GetVolume(); + SetInertia(); + } + } else { + mass = density * GetVolume(); + SetInertia(); + } - // check for negative values - if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0) - throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)", - name.c_str(), id); + // check for negative values + if (mass<0 || inertia[0]<0 || inertia[1]<0 || inertia[2]<0 || density<0) + throw mjCError(this, "mass, inertia or density are negative in geom '%s' (id = %d)", + name.c_str(), id); + } // fluid-interaction coefficients, requires computed inertia and mass if (fluid_switch > 0) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ff49c6b9..3fa44217 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -167,7 +167,7 @@ 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 + // set explicitinertial to true void MakeInertialExplicit(); // variables set by user or 'Compile' @@ -198,7 +198,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 to save the body with an explicit inertial clause + bool explicitinertial; // whether to save the body with an explicit inertial clause int lastdof; // id of last dof (used by compiler) @@ -306,6 +306,7 @@ class mjCGeom : public mjCBase { std::vector userdata; // user data float rgba[4]; // rgba when material is omitted mjtMeshType typeinertia; // selects between surface and volume inertia + bool inferinertia; // true if inertia has to be computed from geom // variables set by user and used during compilation double _mass; // used to compute density @@ -481,7 +482,15 @@ class mjCMesh: public mjCBase { void MakeNormal(void); // compute vertex normals void Process(); // apply transformations void RemoveRepeated(void); // remove repeated vertices - void ComputeInertia(mjtMeshType type); // compute inertia + void ComputeInertia(mjtMeshType type); // compute inertia + void CheckMesh(void); // check if the mesh is valid + + // mesh properties that indicate a well-formed mesh + bool validorientation; // false if mesh have inconsistent faces + bool validarea; // false if the area is too small + bool validvolume; // false if the volume is too small + bool valideigenvalue; // false if inertia eigenvalue is too small + bool validinequality; // false if inertia inequality is not satisfied // mesh properties computed by Compile double pos_volume[3]; // CoM position diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 61be728f..f2fcc6c4 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2284,7 +2284,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { if (pbody->id==0) { throw mjXError(elem, "World body cannot have inertia"); } - pbody->explicit_inertial = true; + pbody->explicitinertial = true; ReadAttr(elem, "pos", 3, pbody->ipos, text, true); ReadAttr(elem, "quat", 4, pbody->iquat, text); ReadAttr(elem, "mass", 1, &pbody->mass, text, true); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index a7e9abdb..886e4a91 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1178,7 +1178,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { WriteVector(elem, "user", body->userdata); // write inertial - if (body->explicit_inertial && + if (body->explicitinertial && model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) { XMLElement* inertial = InsertEnd(elem, "inertial"); WriteAttr(inertial, "pos", 3, body->locipos); diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index e6610d33..d6f1b8ed 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -207,7 +207,7 @@ void mjXURDF::Body(XMLElement* body_elem) { // inertial element: copy into alternative body frame if ((elem = FindSubElem(body_elem, "inertial"))) { - pbody->explicit_inertial = true; + pbody->explicitinertial = true; // origin- relative to joint frame for now Origin(elem, pbody->ipos, pbody->iquat); diff --git a/test/user/testdata/malformed_face.xml b/test/user/testdata/malformed_face.xml index 7539d435..d81edc0e 100644 --- a/test/user/testdata/malformed_face.xml +++ b/test/user/testdata/malformed_face.xml @@ -3,6 +3,8 @@ - + + + diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 84e79847..d61869ec 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -188,11 +188,32 @@ TEST_F(MujocoTest, MalformedFaceFails) { const std::string xml_path = GetTestDataFilePath(kMalformedFaceOBJPath); std::array error; mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); - ASSERT_THAT(model, testing::IsNull()); + EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation")); } TEST_F(MujocoTest, FlippedFaceFails) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation")); +} + +TEST_F(MujocoTest, FlippedFaceAllowedWorld) { static constexpr char xml[] = R"( @@ -206,10 +227,136 @@ TEST_F(MujocoTest, FlippedFaceFails) { )"; std::array error; - LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(error.data(), HasSubstr("faces have inconsistent orientation")); + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()); + mj_deleteModel(model); } +TEST_F(MujocoTest, FlippedFaceAllowedNoMass) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()); + mj_deleteModel(model); +} + +TEST_F(MujocoTest, FlippedFaceAllowedInertial) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()); + mj_deleteModel(model); +} + +TEST_F(MujocoTest, AreaTooSmall) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small")); +} + +TEST_F(MujocoTest, AreaTooSmallAllowedWorld) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()); + mj_deleteModel(model); +} + +TEST_F(MujocoTest, VolumeTooSmall) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); +} + + TEST_F(MujocoTest, VolumeTooSmallAllowedWorld) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()); + mj_deleteModel(model); +} + +// ------------- test concave and shell inertia -------------------------------- + const mjtNum max_abs_err = std::numeric_limits::epsilon(); TEST_F(MujocoTest, ExactConcaveInertia) {