From 41a70499200ba3d8f50f8cbccfa2adb69a77f963 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 10 May 2023 09:11:09 -0700 Subject: [PATCH] Use shell inertia to compute mesh aabb and frame if volume is invalid. PiperOrigin-RevId: 530925161 Change-Id: I94b97efc7198e2b750480df8687e91bc047a6fc9 --- doc/changelog.rst | 8 + src/user/user_mesh.cc | 294 +++++++++++++++++++----------------- src/user/user_objects.h | 6 +- test/user/user_mesh_test.cc | 27 +++- 4 files changed, 192 insertions(+), 143 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 9769bff8..f51e89b8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,14 @@ Upcoming version (not yet released) :align: right :width: 240px + +Bug fixes +^^^^^^^ + +- Fixed a bug that was causing an incorrect computation of the mesh bounding box and coordinate frame if the volume was + invalid. In such case, now MuJoCo only accepts a non-watertight geometry if :ref:`shellinertia` + is equal to ``true`. + Plugins ^^^^^^^ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index ec2ec3ce..90c3f8a8 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -912,137 +912,150 @@ void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type, // apply transformations -void mjCMesh::Process() { - for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) { - double CoM[3] = {0, 0, 0}; - double facecen[3] = {0, 0, 0}; - double area = 0; - double inert[6] = {0, 0, 0, 0, 0, 0}; +void mjCMesh::ApplyTransformations() { + // translate + if (refpos[0]!=0 || refpos[1]!=0 || refpos[2]!=0) { + // prepare translation + float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]}; - double nrm[3]; - double cen[3]; - bool exactmeshinertia = model->exactmeshinertia; + // process vertices + for (int i=0; imjMINVAL) { + float scl = 1/sqrtf(len); + normal[3*i] *= scl; + normal[3*i+1] *= scl; + normal[3*i+2] *= scl; + } else { + normal[3*i] = 0; + normal[3*i+1] = 0; + normal[3*i+2] = 1; + } + } +} - // normalize normals - for (int i=0; imjMINVAL) { - float scl = 1/sqrtf(len); - normal[3*i] *= scl; - normal[3*i+1] *= scl; - normal[3*i+2] *= scl; - } else { - normal[3*i] = 0; - normal[3*i+1] = 0; - normal[3*i+2] = 1; - } - } +// find centroid of faces +void mjCMesh::ComputeFaceCentroid(double facecen[3]) { + double area = 0; + double nrm[3]; + double cen[3]; - // find centroid of faces - for (int i=0; i=nvert) { - throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i); - } - } - - // get area and center - double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]); - - // accumulate - for (int j=0; j<3; j++) { - facecen[j] += a*cen[j]; - } - area += a; - } - - // require positive area - if (area < mjMINVAL) { - validarea = false; - return; - } - - // finalize centroid of faces - for (int j=0; j<3; j++) { - facecen[j] /= area; + for (int i=0; i=nvert) { + throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i); } } + // get area and center + double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]); + + // accumulate + for (int j=0; j<3; j++) { + facecen[j] += a*cen[j]; + } + area += a; + } + + // require positive area + if (area < mjMINVAL) { + validarea = false; + return; + } + + // finalize centroid of faces + for (int j=0; j<3; j++) { + facecen[j] /= area; + } +} + + +void mjCMesh::Process() { + double facecen[3] = {0, 0, 0}; + double nrm[3]; + double cen[3]; + + // user offset, rotation, scaling + ApplyTransformations(); + + // find centroid of faces + ComputeFaceCentroid(facecen); + + // compute inertial properties for both inertia types + for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) { + double CoM[3] = {0, 0, 0}; + double inert[6] = {0, 0, 0, 0, 0, 0}; + bool exactmeshinertia = model->exactmeshinertia; + // compute CoM and volume from pyramid volumes ComputeVolume(CoM, type, facecen, model->exactmeshinertia); - // perform computation again if volume is negative - if (GetVolumeRef(type) < mjMINVAL && exactmeshinertia) { + // perform computation with convex mesh if volume is negative + if (GetVolumeRef(type) <= 0 && exactmeshinertia) { mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str()); exactmeshinertia = false; ComputeVolume(CoM, type, facecen, exactmeshinertia); } - - // require positive volume + // if volume is still invalid, skip the rest of the computations if (GetVolumeRef(type) < mjMINVAL) { validvolume = GetVolumeRef(type) < 0 ? -1 : 0; - return; + continue; } // finalize CoM, save as mesh center @@ -1052,7 +1065,7 @@ void mjCMesh::Process() { mjuu_copyvec(GetPosPtr(type), CoM, 3); // re-center mesh at CoM - if (type==mjVOLUME_MESH) { + if (type==mjVOLUME_MESH || validvolume<=0) { for (int i=0; i0) { + mju_copy4(GetQuatPtr(type), GetQuatPtr(mjVOLUME_MESH)); + continue; } // rotate vertices and normals into axis-aligned frame - if (type==mjVOLUME_MESH) { - double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]}; - double mat[9]; - mjuu_quat2mat(mat, neg); - for (int i=0; i - + @@ -544,6 +544,31 @@ TEST_F(MjCMeshTest, VolumeTooSmall) { EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); } +TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, testing::NotNull()); + EXPECT_LE(mju_abs(model->geom_size[0]), 1); + EXPECT_LE(mju_abs(model->geom_size[1]), 1); + EXPECT_LE(mju_abs(model->geom_size[2]), 1); + mj_deleteModel(model); +} + TEST_F(MjCMeshTest, VolumeNegativeDefaultsLegacy) { static constexpr char xml[] = R"(