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) {