Skip to content

Commit 78a4d2f

Browse files
powertjcopybara-github
powertj
authored and
copybara-github
committed
Fix mass/inertia checking for moving bodies with nested children.
Previously, we only checked immediate children of moving bodies. PiperOrigin-RevId: 726999705 Change-Id: I133a557daf86223b8751c6d46e25809781e168a1
1 parent 37d7591 commit 78a4d2f

File tree

3 files changed

+128
-27
lines changed

3 files changed

+128
-27
lines changed

‎src/user/user_model.cc‎

Lines changed: 45 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -4319,33 +4319,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
43194319
}
43204320
}
43214321

4322-
// check body mass and inertia
4323-
for (int i=1; i<bodies_.size(); i++) {
4324-
mjCBody* b = bodies_[i];
4325-
4326-
// find moving body with small mass or inertia
4327-
if (!b->joints.empty() &&
4328-
(b->mass<mjMINVAL ||
4329-
b->inertia[0]<mjMINVAL ||
4330-
b->inertia[1]<mjMINVAL ||
4331-
b->inertia[2]<mjMINVAL)) {
4332-
// does it have static children with mass and inertia
4333-
bool ok = false;
4334-
for (size_t j=0; j<b->bodies.size(); j++) {
4335-
if (b->bodies[j]->joints.empty() &&
4336-
b->bodies[j]->mass>=mjMINVAL &&
4337-
b->bodies[j]->inertia[0]>=mjMINVAL &&
4338-
b->bodies[j]->inertia[1]>=mjMINVAL &&
4339-
b->bodies[j]->inertia[2]>=mjMINVAL) {
4340-
ok = true;
4341-
break;
4342-
}
4343-
}
4344-
4345-
// error
4346-
if (!ok) {
4347-
throw mjCError(b, "mass and inertia of moving bodies must be larger than mjMINVAL");
4348-
}
4322+
// check mass and inertia of moving bodies
4323+
if (bodies_.size() > 1) {
4324+
// we ignore the first body as it is the world body
4325+
if (!CheckBodiesMassInertia(std::vector<mjCBody*>(bodies_.begin()+1, bodies_.end()))) {
4326+
throw mjCError(0, "mass and inertia of moving bodies must be larger than mjMINVAL");
43494327
}
43504328
}
43514329

@@ -4497,6 +4475,46 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
44974475

44984476

44994477

4478+
bool mjCModel::CheckBodiesMassInertia(std::vector<mjCBody*> bodies) {
4479+
// check mass and inertia of moving bodies
4480+
for (int i=0; i<bodies.size(); i++) {
4481+
if (!bodies[i]->joints.empty()) {
4482+
if (!CheckBodyMassInertia(bodies[i])) {
4483+
return false;
4484+
}
4485+
}
4486+
}
4487+
return true;
4488+
}
4489+
4490+
4491+
4492+
bool mjCModel::CheckBodyMassInertia(mjCBody* body) {
4493+
// check if body has valid mass and inertia
4494+
if (body->mass>=mjMINVAL &&
4495+
body->inertia[0]>=mjMINVAL &&
4496+
body->inertia[1]>=mjMINVAL &&
4497+
body->inertia[2]>=mjMINVAL) {
4498+
return true;
4499+
}
4500+
4501+
// body is valid if we find a single static child with valid mass and inertia
4502+
for (int i=0; i<body->Bodies().size(); i++) {
4503+
// if we find a child with a joint, time to move on to the next moving body
4504+
if (!body->Bodies()[i]->joints.empty()) {
4505+
continue;
4506+
}
4507+
if (CheckBodyMassInertia(body->Bodies()[i])) {
4508+
return true;
4509+
}
4510+
}
4511+
4512+
// we did not find a child with valid mass and inertia
4513+
return false;
4514+
}
4515+
4516+
4517+
45004518
//------------------------------- DECOMPILER -------------------------------------------------------
45014519

45024520
// get numeric data back from mjModel

‎src/user/user_model.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -414,6 +414,13 @@ class mjCModel : public mjCModel_, private mjSpec {
414414
// compute qpos0
415415
void ComputeReference();
416416

417+
// return true if all bodies have valid mass and inertia
418+
bool CheckBodiesMassInertia(std::vector<mjCBody*> bodies);
419+
420+
// return true if body has valid mass and inertia
421+
bool CheckBodyMassInertia(mjCBody* body);
422+
423+
417424
mjListKeyMap ids; // map from object names to ids
418425
mjCError errInfo; // last error info
419426
std::vector<mjKeyInfo> key_pending_; // attached keyframes

‎test/user/user_model_test.cc‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -140,6 +140,82 @@ TEST_F(UserCModelTest, ActuatorSparsity) {
140140
mj_deleteModel(m);
141141
}
142142

143+
TEST_F(UserCModelTest, NestedZeroMassBodiesOK) {
144+
static constexpr char xml[] = R"(
145+
<mujoco>
146+
<worldbody>
147+
<body>
148+
<freejoint/>
149+
<body>
150+
<body>
151+
<body>
152+
<geom size="1"/>
153+
</body>
154+
</body>
155+
</body>
156+
</body>
157+
</worldbody>
158+
</mujoco>
159+
)";
160+
char error[1024];
161+
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
162+
ASSERT_THAT(model, NotNull()) << error;
163+
mj_deleteModel(model);
164+
}
165+
166+
TEST_F(UserCModelTest, NestedZeroMassBodiesWithJointOK) {
167+
static constexpr char xml[] = R"(
168+
<mujoco>
169+
<worldbody>
170+
<body>
171+
<freejoint/>
172+
<body>
173+
<body>
174+
<body>
175+
<joint/>
176+
<geom size="1"/>
177+
</body>
178+
<body>
179+
<geom size="1"/>
180+
</body>
181+
</body>
182+
</body>
183+
</body>
184+
</worldbody>
185+
</mujoco>
186+
)";
187+
char error[1024];
188+
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
189+
ASSERT_THAT(model, NotNull()) << error;
190+
mj_deleteModel(model);
191+
}
192+
193+
TEST_F(UserCModelTest, NestedZeroMassBodiesFail) {
194+
static constexpr char xml[] = R"(
195+
<mujoco>
196+
<worldbody>
197+
<body>
198+
<geom size="1"/>
199+
<body>
200+
<freejoint/>
201+
<body>
202+
<body>
203+
</body>
204+
</body>
205+
</body>
206+
</body>
207+
</worldbody>
208+
</mujoco>
209+
)";
210+
char error[1024];
211+
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
212+
ASSERT_THAT(model, IsNull());
213+
EXPECT_THAT(
214+
error,
215+
HasSubstr(
216+
"mass and inertia of moving bodies must be larger than mjMINVAL"));
217+
mj_deleteModel(model);
218+
}
143219

144220
// ------------- test automatic inference of nuser_xxx -------------------------
145221

0 commit comments

Comments
 (0)