From 6ba4d6f0de482fd1d5f1ef1b54e2ba2cd04ca6bc Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 17 Jan 2023 10:30:58 -0800 Subject: [PATCH] Fix a bug that was causing a mismatch between the orientation of the mesh frame and the vertex coordinates. PiperOrigin-RevId: 502624466 Change-Id: Ib28fd2a118b6572be98fadb078fa2262b21cbfc0 --- doc/changelog.rst | 3 +++ src/user/user_mesh.cc | 2 +- test/user/testdata/torus_shell.xml | 8 ++++++++ test/user/user_mesh_test.cc | 17 +++++++++++++++++ 4 files changed, 29 insertions(+), 1 deletion(-) create mode 100644 test/user/testdata/torus_shell.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index 57f94107..d2d2e09b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -18,6 +18,9 @@ General Bug fixes ^^^^^^^^^ +- Fixed a bug that for :at:`shellinertia` equal to ``true`` caused the mesh orientation to be overwritten by the + principal components of the shell inertia, while the vertex coordinates are rotated using the volumetric inertia. + Now the volumetric inertia orientation is used also in the shell case. - Fixed misalignment bug in mesh-to-primitive fitting when using the bounding box fitting option :at:`fitaabb`. - The ``launch_repl`` functionality in the Python viewer is fixed. diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index f2191b43..0e50c231 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -1152,7 +1152,7 @@ void mjCMesh::Process() { // copy quat for (j=0; j<4; j++) { - GetQuatPtr(type)[j] = quattmp[j]; + GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j]; } // rotate vertices and normals into axis-aligned frame diff --git a/test/user/testdata/torus_shell.xml b/test/user/testdata/torus_shell.xml new file mode 100644 index 00000000..b31a6039 --- /dev/null +++ b/test/user/testdata/torus_shell.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index b3b12611..b91be472 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -38,6 +38,8 @@ static const char* const kCubePath = "user/testdata/cube.xml"; static const char* const kTorusPath = "user/testdata/torus.xml"; +static const char* const kTorusShellPath = + "user/testdata/torus_shell.xml"; static const char* const kConvexInertiaPath = "user/testdata/inertia_convex.xml"; static const char* const kConcaveInertiaPath = @@ -352,6 +354,21 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, ShellUsesVolumeFrame) { + const std::string xml_path_v = GetTestDataFilePath(kTorusPath); + const std::string xml_path_s = GetTestDataFilePath(kTorusShellPath); + std::array error; + mjModel* mv = mj_loadXML(xml_path_v.c_str(), 0, error.data(), error.size()); + mjModel* ms = mj_loadXML(xml_path_s.c_str(), 0, error.data(), error.size()); + mjtNum tolerance = std::numeric_limits::epsilon(); + EXPECT_NEAR(mv->geom_quat[0], ms->geom_quat[0], tolerance); + EXPECT_NEAR(mv->geom_quat[1], ms->geom_quat[1], tolerance); + EXPECT_NEAR(mv->geom_quat[2], ms->geom_quat[2], tolerance); + EXPECT_NEAR(mv->geom_quat[3], ms->geom_quat[3], tolerance); + mj_deleteModel(mv); + mj_deleteModel(ms); +} + TEST_F(MjCMeshTest, AreaTooSmall) { static constexpr char xml[] = R"(