From 50f43b8745298cc9e8aa5ce7b90383d821f074b2 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 20 Sep 2024 07:52:31 -0700 Subject: [PATCH] Fix various issues with body inertial parsing and compiling. - Add errors in body compiler for incorrect mixing of specifiers. - Correctly process inertia orientation specifiers. PiperOrigin-RevId: 676843326 Change-Id: I7e7eb22e25a6769754794f98fad5e0dcc0e5946f --- src/user/user_objects.cc | 33 +++++++++++++---- src/xml/xml_native_reader.cc | 2 +- src/xml/xml_urdf.cc | 4 +- test/user/user_objects_test.cc | 59 ++++++++++++++++++++++++++++++ test/xml/xml_native_reader_test.cc | 2 +- 5 files changed, 89 insertions(+), 11 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index d7ebe82f..22d00ae2 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1497,15 +1497,34 @@ void mjCBody::Compile(void) { } // check and process orientation alternatives for body - const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); - if (err) { - throw mjCError(this, "error '%s' in frame alternative", err); + if (alt.type != mjORIENTATION_QUAT) { + const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); + if (err) { + throw mjCError(this, "error '%s' in frame alternative", err); + } } - // check and process orientation alternatives for inertia - const char* ierr = mjuu_fullInertia(iquat, inertia, this->fullinertia); - if (ierr) { - throw mjCError(this, "error '%s' in inertia alternative", ierr); + // check orientation alternatives for inertia + if (mjuu_defined(fullinertia[0]) && ialt.type != mjORIENTATION_QUAT) { + throw mjCError(this, "fullinertia and inertial orientation cannot both be specified"); + } + if (mjuu_defined(fullinertia[0]) && (inertia[0] || inertia[1] || inertia[2])) { + throw mjCError(this, "fullinertia and diagonal inertia cannot both be specified"); + } + + // process orientation alternatives for inertia + if (mjuu_defined(fullinertia[0])) { + const char* err = mjuu_fullInertia(iquat, inertia, this->fullinertia); + if (err) { + throw mjCError(this, "error '%s' in fullinertia", err); + } + } + + if (ialt.type != mjORIENTATION_QUAT) { + const char* err = ResolveOrientation(iquat, model->degree, model->eulerseq, ialt); + if (err) { + throw mjCError(this, "error '%s' in inertia alternative", err); + } } // compile all geoms diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index bad6fa41..ed6e4339 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -3443,7 +3443,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, bool alt = ReadAlternative(elem, body->ialt); bool full = ReadAttr(elem, "fullinertia", 6, body->fullinertia, text); if (alt && full) { - throw mjXError(elem, "fullinertia and orientation specifiers cannot be used together"); + throw mjXError(elem, "fullinertia and inertial orientation cannot both be specified"); } } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index a0fc21d6..3dc98b65 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -274,9 +274,9 @@ void mjXURDF::Body(XMLElement* body_elem) { // lquat = rotation from specified to default (joint/body) inertial frame double lquat[4] = {1, 0, 0, 0}; double tmpquat[4] = {1, 0, 0, 0}; - const char* altres = mjuu_fullInertia(lquat, pbody->inertia, pbody->fullinertia); + const char* altres = mjuu_fullInertia(lquat, nullptr, pbody->fullinertia); - // inertia are sometimes 0 in URDF files: ignore error in altres, fix later + // inertias are sometimes 0 in URDF files: ignore error in altres, fix later (void) altres; // correct for alignment of full inertia matrix diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 30576c3b..2a97ec58 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -2487,5 +2487,64 @@ TEST_F(UserObjectsTest, BadWeld) { mj_deleteSpec(s); } +TEST_F(UserObjectsTest, Inertial) { + string xml = R"( + + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + EXPECT_EQ(m->body_mass[1], 1); + EXPECT_THAT(AsVector(m->body_ipos+3, 3), ElementsAre(2, 3, 4)); + EXPECT_THAT(AsVector(m->body_inertia+3, 3), ElementsAre(4, 5, 6)); + + mjtNum quat[4]; + const mjtNum euler[3] = {3, 4, 5}; + mju_euler2Quat(quat, euler, "xyz"); + EXPECT_EQ(AsVector(m->body_iquat+4, 4), AsVector(quat, 4)); + + EXPECT_EQ(m->body_mass[2], 2); + EXPECT_THAT(AsVector(m->body_ipos+6, 3), ElementsAre(1, 2, 3)); + EXPECT_THAT(AsVector(m->body_inertia+6, 3), ElementsAre(4, 3, 2)); + mj_deleteModel(m); + + string bad_xml1 = R"( + + + + + + + + )"; + m = LoadModelFromString(bad_xml1.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr("fullinertia and diagonal inertia cannot both")); + + string bad_xml2 = R"( + + + + + + + + )"; + m = LoadModelFromString(bad_xml2.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot")); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index c5ba1556..df24d742 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1640,7 +1640,7 @@ TEST_F(XMLReaderTest, InvalidInertialOrientation) { EXPECT_THAT( error.data(), HasSubstr( - "fullinertia and orientation specifiers cannot be used together")); + "fullinertia and inertial orientation cannot both be specified")); } TEST_F(XMLReaderTest, ReadShellParameter) {