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

Add nested keyframes attachment test.

Browse Source 
This is a no-op change for keyframes but it shows an example of behavior.

Also, fixed two bugs:
- Missing namespace when attaching frames.
- Incorrect body id and subtreedof when attaching a compiled spec. Fixes #2063.

PiperOrigin-RevId: 680289467
Change-Id: I2ddaa7ad2226fa466ec3bbf74a7553d497bb4bb9
This commit is contained in:
Alessio Quaglino
2024-09-29 11:56:50 -07:00
committed by Copybara-Service
parent 807ee852d0
commit 58a49bad96
3 changed files with 116 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+8
View File
@@ -119,6 +119,10 @@ mjModel* mj_compile(mjSpec* s, const mjVFS* vfs) {
// attach body to a frame of the parent
mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child,
const char* prefix, const char* suffix) {
if (!parent) {
mju_error("parent frame is null");
return nullptr;
}
mjCFrame* frame_parent = static_cast<mjCFrame*>(parent->element);
mjCBody* child_body = static_cast<mjCBody*>(child->element);
try {
@@ -137,6 +141,10 @@ mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child,
// attach frame to a parent body
mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
const char* prefix, const char* suffix) {
if (!parent) {
mju_error("parent body is null");
return nullptr;
}
mjCBody* body_parent = static_cast<mjCBody*>(parent->element);
mjCFrame* child_frame = static_cast<mjCFrame*>(child->element);
try {
src/user/user_objects.cc
+6 -1
View File
@@ -802,7 +802,8 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
sites.clear();
cameras.clear();
lights.clear();
id = other.id;
id = -1;
subtreedofs = 0;
// add elements to lists
*this += other;
@@ -1040,6 +1041,10 @@ void mjCBody::NameSpace_(const mjCModel* m, bool propagate) {
for (auto& light : lights) {
light->NameSpace(m);
}
for (auto& frame : frames) {
frame->NameSpace(m);
}
}
test/user/user_api_test.cc
+102
View File
@@ -1284,5 +1284,107 @@ TEST_F(MujocoTest, InitTexture) {
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, AttachNestedKeyframe) {
static constexpr char parent_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
<frame name="frame"/>
</worldbody>
</mujoco>)";
static constexpr char child_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".2"/>
<frame name="frame"/>
</body>
</worldbody>
<keyframe>
<key name="key2" time="2" qpos="2"/>
</keyframe>
</mujoco>)";
static constexpr char gchild_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".3"/>
</body>
</worldbody>
<keyframe>
<key name="key1" time="1" qpos="1"/>
</keyframe>
</mujoco>)";
static constexpr char expected_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
<frame name="frame">
<body name="child-body">
<joint name="child-joint" type="slide"/>
<geom size=".2"/>
<frame name="child-frame">
<body name="child-gchild-body">
<joint name="child-gchild-joint" type="slide"/>
<geom size=".3"/>
</body>
</frame>
</body>
</frame>
</worldbody>
<keyframe>
<key name="child-key2" time="2" qpos="0 2 0"/>
<key name="child-gchild-key1" time="1" qpos="0 0 1"/>
</keyframe>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* parent = mj_parseXMLString(parent_xml, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(child_xml, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, er.data(), er.size());
EXPECT_THAT(gchild, NotNull()) << er.data();
// attach gchild to child
mjs_attachBody(mjs_findFrame(child, "frame"),
mjs_findBody(gchild, "body"), "gchild-", "");
// compile required before further attachment
mjModel* m_child = mj_compile(child, 0);
// attach child to parent
mjs_attachBody(mjs_findFrame(parent, "frame"),
mjs_findBody(child, "body"), "child-", "");
mjtNum tol = 0;
std::string field = "";
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
mjModel* m_expected = LoadModelFromString(expected_xml, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';;
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteSpec(gchild);
mj_deleteModel(m_expected);
mj_deleteModel(m_attached);
mj_deleteModel(m_child);
}
} // namespace
} // namespace mujoco