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

Add body/simple attribute to control simple body optimization.

Browse Source 
PiperOrigin-RevId: 942164766
Change-Id: I1e83bb99b6a1955917eb9b2e9bed0dc7b8e6a161
This commit is contained in:
Yuval Tassa
2026-07-03 09:08:42 -07:00
committed by Copybara-Service
parent 4ecfe5c1f3
commit 5618666a7d
19 changed files with 205 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_setconst_test.cc
+37 -6
View File
@@ -329,10 +329,12 @@ TEST_F(SetConstTest, TendonTreeId) {
int t_static_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_static");
int t_tree1_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_tree1");
int t_intertree12_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree12");
int t_intertree123_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree123");
int b1_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")];
int b2_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B2_1")];
int t_intertree123_id =
mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree123");
int b1_1_treeid =
model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")];
int b2_1_treeid =
model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B2_1")];
// Tendon 1: Not associated with any tree
EXPECT_EQ(model->tendon_treenum[t_static_id], 0);
@@ -461,7 +463,7 @@ TEST_F(SetConstTest, BodySameframeRecomputed) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<body name="B1" simple="false">
<joint type="slide"/>
<geom size=".1"/>
</body>
@@ -570,7 +572,7 @@ TEST_F(SetConstTest, SameframeKinematicsCorrect) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1" pos="1 0 0">
<body name="B1" pos="1 0 0" simple="false">
<joint type="slide" axis="1 0 0"/>
<geom name="G1" size=".1"/>
<site name="S1"/>
@@ -604,5 +606,34 @@ TEST_F(SetConstTest, SameframeKinematicsCorrect) {
EXPECT_NEAR(d->geom_xpos[3*g+2], 0.3, MjTol(1e-10, 1e-6));
}
TEST_F(SetConstTest, SimpleBodyLostSameframeError) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d(mj_makeData(m.get()));
int b = mj_name2id(m.get(), mjOBJ_BODY, "B1");
// confirm body is compiled as simple
EXPECT_GT(m->body_simple[b], 0);
// perturb body_ipos, breaking sameframe; calling mj_setConst should fail
m->body_ipos[3*b+0] = 1.0;
std::string err = MjuErrorMessageFrom(mj_setConst)(m.get(), d.get());
EXPECT_THAT(err, HasSubstr("body 1 is compiled as simple but "
"sameframe no longer holds"));
}
} // namespace
} // namespace mujoco
test/mjspecmacro_test.cc
+1 -1
View File
@@ -140,7 +140,7 @@ CHECK_STRUCT(mjsDefault, MJSDEFAULT_FIELDS)
#undef XIMPL
TEST(MjspecmacroTest, CompileTimeChecks) {
// Verifies that the test binary compiled successfully and all static_asserts passed.
// Verifies that the test compiled successfully and all static_asserts passed.
EXPECT_TRUE(true);
}
test/user/user_objects_test.cc
+49
View File
@@ -1287,6 +1287,55 @@ TEST_F(MjCJointTest, AlignFree) {
mj_deleteSpec(s);
}
TEST_F(MjCJointTest, BodySimpleFalse) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
constexpr char xml_nosimple[] = R"(
<mujoco>
<worldbody>
<body name="B1" simple="false">
<joint type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
// compile default model: body should be simple
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
int b = mj_name2id(m.get(), mjOBJ_BODY, "B1");
EXPECT_GT(m->body_simple[b], 0);
EXPECT_GT(m->dof_simplenum[0], 0);
// compile with simple="false": body should not be simple
MjModelPtr m_ns = LoadModelFromString(xml_nosimple, error, sizeof(error));
ASSERT_THAT(m_ns.get(), NotNull()) << error;
EXPECT_EQ(m_ns->body_simple[b], 0);
EXPECT_EQ(m_ns->dof_simplenum[0], 0);
// forward kinematics should produce identical results
MjDataPtr d = MakeData(m);
MjDataPtr d_ns = MakeData(m_ns);
d->qpos[0] = d_ns->qpos[0] = 0.5;
mj_forward(m.get(), d.get());
mj_forward(m_ns.get(), d_ns.get());
EXPECT_THAT(d_ns->xpos[3*b+0], MjNear(d->xpos[3*b+0], 1e-10, 1e-6));
EXPECT_THAT(d_ns->xpos[3*b+1], MjNear(d->xpos[3*b+1], 1e-10, 1e-6));
EXPECT_THAT(d_ns->xpos[3*b+2], MjNear(d->xpos[3*b+2], 1e-10, 1e-6));
}
// ------------- test height fields --------------------------------------------
using MjCHFieldTest = MujocoTest;
test/xml/xml_native_writer_test.cc
+36
View File
@@ -1590,5 +1590,41 @@ TEST_F(XMLWriterTest, WritesActuatorDelayHistory) {
EXPECT_THAT(saved_xml, HasSubstr("interp=\"cubic\""));
}
TEST_F(XMLWriterTest, BodySimpleRoundtrip) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1" simple="false">
<joint type="ball"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(xml);
ASSERT_THAT(model.get(), NotNull());
std::string saved_xml = SaveAndReadXml(model.get());
EXPECT_THAT(saved_xml, HasSubstr("simple=\"false\""));
// auto should not be written
static constexpr char xml_auto[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint type="ball"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
MjModelPtr model_auto = LoadModelFromString(xml_auto);
ASSERT_THAT(model_auto.get(), NotNull());
std::string saved_auto = SaveAndReadXml(model_auto.get());
EXPECT_THAT(saved_auto, Not(HasSubstr("simple=")));
// nC should increase when simple is disabled
EXPECT_GT(model->nC, model_auto->nC);
}
} // namespace
} // namespace mujoco