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

Add mjs_attachToSite for attaching a body to a site.

Browse Source 
PiperOrigin-RevId: 684432183
Change-Id: I7100bebfb6521f9dbfbb07f31490b2b9298bf18c
This commit is contained in:
Alessio Quaglino
2024-10-10 07:11:16 -07:00
committed by Copybara-Service
parent 694bdebc19
commit cdb167f275
10 changed files with 180 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/user/user_api_test.cc
+63
View File
@@ -1079,6 +1079,69 @@ TEST_F(MujocoTest, DetachBody) {
TestDetachBody(/*compile=*/true);
}
TEST_F(MujocoTest, AttachToSite) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<site name="site" pos="1 0 0" quat="0 1 0 0"/>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco>
<worldbody>
<site name="site" pos="1 0 0" quat="0 1 0 0"/>
<frame pos="1 0 0" quat="0 1 0 0">
<body name="attached-sphere-1">
<joint type="slide"/>
<geom size=".1"/>
</body>
</frame>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjsBody* world = mjs_findBody(parent, "world");
EXPECT_THAT(world, NotNull());
mjsSite* site = mjs_asSite(mjs_firstChild(world, mjOBJ_SITE, 0));
EXPECT_THAT(site, NotNull());
mjsBody* body = mjs_findBody(child, "sphere");
EXPECT_THAT(body, NotNull());
mjsBody* attached = mjs_attachToSite(site, body, "attached-", "-1");
EXPECT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
mj_deleteModel(expected);
}
TEST_F(MujocoTest, PreserveState) {
std::array<char, 1000> er;
std::string field = "";