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

Clear body subtreedofs when compiling. Fixes issue when attaching previously compiled model.

Browse Source 
PiperOrigin-RevId: 738906593
Change-Id: I74875b4d07c422e397358dff5daf6682ec1bd655
This commit is contained in:
Tom Power
2025-03-20 12:51:33 -07:00
committed by Copybara-Service
parent 2e11afa2de
commit 8d92a0fbe6
2 changed files with 60 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+5 -3
View File
@@ -4008,9 +4008,6 @@ static void warninghandler(const char* msg) {
mjModel* mjCModel::Compile(const mjVFS* vfs, mjModel** m) {
if (compiled) {
// clear kinematic tree
for (int i=0; i < bodies_.size(); i++) {
bodies_[i]->subtreedofs = 0;
}
mjCBody* world = bodies_[0];
ResetTreeLists();
Clear();
@@ -4313,6 +4310,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// make lists of objects created in kinematic tree
MakeLists(bodies_[0]);
// clear subtreedofs
for (int i=0; i < bodies_.size(); i++) {
bodies_[i]->subtreedofs = 0;
}
// fill missing names and check that they are all filled
for (const auto& asset : meshes_) asset->CopyFromSpec();
for (const auto& asset : skins_) asset->CopyFromSpec();
test/user/user_api_test.cc
+55
View File
@@ -1220,6 +1220,61 @@ TEST_F(MujocoTest, AttachFrame) {
mj_deleteModel(m_expected);
}
TEST_F(MujocoTest, AttachCompiled) {
std::array<char, 1000> er;
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane"/>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="base">
<freejoint/>
<geom name="geom1" size="0.1" type="sphere"/>
</body>
</worldbody>
</mujoco>)";
// load parent
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
// load child
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
// compile child
mjModel* m_child = mj_compile(child, 0);
EXPECT_THAT(m_child, NotNull()) << mjs_getError(child);
// add frame to the parent to attach
mjsBody* world = mjs_findBody(parent, "world");
EXPECT_THAT(world, NotNull()) << mjs_getError(parent);
mjsFrame* frame = mjs_addFrame(world, 0);
EXPECT_THAT(frame, NotNull()) << mjs_getError(parent);
// attach child body to the frame
mjsBody* to_attach = mjs_findBody(child, "base");
EXPECT_THAT(to_attach, NotNull()) << mjs_getError(child);
mjs_attachBody(frame, to_attach, "", "");
// check that attached model can be compiled
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull()) <<
"Failed to compile attached model" << mjs_getError(parent);
// destroy everything
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(m_attached);
mj_deleteModel(m_child);
}
void TestDetachBody(bool compile) {
std::array<char, 1000> er;
mjtNum tol = 0;