chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ignore geoms with tiny mass in inertiafromgeom computation. Fixes #2754

Browse Source 
PiperOrigin-RevId: 795475978
Change-Id: I02b5db9e2ac7ec556cb0a0ec6dbe4214ba9128d7
This commit is contained in:
Yuval Tassa
2025-08-15 07:56:14 -07:00
committed by Copybara-Service
parent 977f94e9df
commit b566ef2883
2 changed files with 65 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+3 -2
View File
@@ -1902,11 +1902,12 @@ void mjCBody::InertiaFromGeom(void) {
double toti[6] = {0, 0, 0, 0, 0, 0};
std::vector<mjCGeom*> sel;
// select geoms based on group
// select geoms based on group, ignore tiny masses
sel.clear();
for (int i=0; i < geoms.size(); i++) {
if (geoms[i]->group >= compiler->inertiagrouprange[0] &&
geoms[i]->group <= compiler->inertiagrouprange[1]) {
geoms[i]->group <= compiler->inertiagrouprange[1] &&
geoms[i]->mass_ > mjEPS) {
sel.push_back(geoms[i]);
}
}
test/user/user_objects_test.cc
+38 -27
View File
@@ -68,8 +68,7 @@ TEST_F(VfsTest, HFieldPngWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -96,8 +95,7 @@ TEST_F(VfsTest, HFieldCustomWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -125,8 +123,7 @@ TEST_F(VfsTest, TexturePngWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -154,8 +151,7 @@ TEST_F(VfsTest, TextureCustomWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -187,8 +183,7 @@ TEST_F(ContentTypeTest, HFieldPngWithContentType) {
// should try loading the file
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -216,8 +211,7 @@ TEST_F(ContentTypeTest, HFieldCustomWithContentType) {
// should try loading the file
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -273,8 +267,7 @@ TEST_F(ContentTypeTest, TexturePngWithContentType) {
// should try loading the file
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -303,8 +296,7 @@ TEST_F(ContentTypeTest, TextureCustomWithContentType) {
// should try loading the file
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error,
HasSubstr("Error opening file"));
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
@@ -369,7 +361,6 @@ TEST_F(ContentTypeTest, TextureLoadPng) {
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
@@ -607,9 +598,7 @@ TEST_F(RelativeFrameSensorParsingTest, BadObjRefName) {
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(
error.data(),
HasSubstr("unrecognized name 'alessio' of object"));
EXPECT_THAT(error.data(), HasSubstr("unrecognized name 'alessio' of object"));
EXPECT_THAT(error.data(), HasSubstr("name 'tom'"));
EXPECT_THAT(error.data(), HasSubstr("line 7"));
}
@@ -1199,7 +1188,8 @@ TEST_F(MjCJointTest, AlignFree) {
// expect x-frames (sensors) to match to very high precision
double eps = 1e-10;
EXPECT_THAT(AsVector(d->sensordata, m->nsensordata),
EXPECT_THAT(
AsVector(d->sensordata, m->nsensordata),
Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
// no frame sensors for lights, test separately
@@ -1224,10 +1214,10 @@ TEST_F(MjCJointTest, AlignFree) {
EXPECT_GT(mju_norm(dqpos, nq), 1.0);
mj_freeStack(d);
// expect x-frames to match to reasonable precision
eps = 1e-5;
EXPECT_THAT(AsVector(d->sensordata, m->nsensordata),
EXPECT_THAT(
AsVector(d->sensordata, m->nsensordata),
Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
EXPECT_THAT(AsVector(d->light_xpos, 3),
Pointwise(DoubleNear(eps), AsVector(d_u->light_xpos, 3)));
@@ -1247,8 +1237,7 @@ TEST_F(MjCJointTest, AlignFree) {
using MjCHFieldTest = MujocoTest;
TEST_F(MjCHFieldTest, PngMap) {
const string xml_path =
GetTestDataFilePath("user/testdata/hfield_png.xml");
const string xml_path = GetTestDataFilePath("user/testdata/hfield_png.xml");
std::array<char, 1024> error;
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size());
@@ -2402,7 +2391,8 @@ TEST_F(UserObjectsTest, BadConnect) {
EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0));
mj_deleteModel(m);
char error_missing[] = "either both body1 and anchor must be defined,"
char error_missing[] =
"either both body1 and anchor must be defined,"
" or both site1 and site2 must be defined\nElement 'connect', line 12";
// bad model (missing anchor)
@@ -2411,7 +2401,8 @@ TEST_F(UserObjectsTest, BadConnect) {
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_missing));
char error_mixed[] = "body and site semantics cannot be mixed"
char error_mixed[] =
"body and site semantics cannot be mixed"
"\nElement 'connect', line 12";
// bad model (mixing body and site)
@@ -2611,5 +2602,25 @@ TEST_F(UserObjectsTest, Inertial) {
EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot"));
}
TEST_F(UserObjectsTest, ZeroMass) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<body>
<geom mass="0" size="0.01"/>
<geom mass="0" size="0.01"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco