diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 6220d98a..f06d3de2 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -91,6 +91,17 @@ int mjs_attachBody(mjsFrame* parent, const mjsBody* child, +// attach frame to a parent body +int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, + const char* prefix, const char* suffix) { + mjCBody* body_parent = static_cast(parent->element); + mjCFrame* child_frame = static_cast(child->element); + *body_parent += std::string(prefix) + *child_frame + std::string(suffix); + return 0; +} + + + // get error message from model const char* mjs_getError(mjSpec* s) { mjCModel* modelC = static_cast(s->element); diff --git a/src/user/user_api.h b/src/user/user_api.h index dd03c959..40907f47 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -750,6 +750,10 @@ MJAPI mjModel* mjs_compile(mjSpec* s, const mjVFS* vfs); MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child, const char* prefix, const char* suffix); +// Attach child frame to a parent body, return 0 if success +MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, + const char* prefix, const char* suffix); + // Get error message from model. MJAPI const char* mjs_getError(mjSpec* s); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 17421d9a..9f9053f6 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -729,6 +729,49 @@ mjCBody& mjCBody::operator+=(const mjCBody& other) { +// attach frame to body +mjCBody& mjCBody::operator+=(const mjCFrame& other) { + mjCBody* subtree = other.body; + other.model->prefix = other.prefix; + other.model->suffix = other.suffix; + + // map old frames to indices + std::map fmap; + for (int i=0; iframes.size(); i++) { + fmap[subtree->frames[i]] = i; + } + + // copy children that are inside the input frame + CopyList(frames, subtree->frames, fmap, &other); // needs to be done first + CopyList(geoms, subtree->geoms, fmap, &other); + CopyList(joints, subtree->joints, fmap, &other); + CopyList(sites, subtree->sites, fmap, &other); + CopyList(cameras, subtree->cameras, fmap, &other); + CopyList(lights, subtree->lights, fmap, &other); + + for (int i=0; ibodies.size(); i++) { + if (subtree->bodies[i]->frame != &other) { + continue; + } + bodies.push_back(new mjCBody(*subtree->bodies[i], model)); // triggers recursive call + bodies.back()->frame = + subtree->bodies[i]->frame ? frames[fmap[subtree->bodies[i]->frame]] : nullptr; + } + + // name space + this->NameSpace(other.model); + + // attach referencing elements + *model += *other.model; + + // clear namespace and return body + other.model->prefix.clear(); + other.model->suffix.clear(); + return *this; +} + + + // copy src list of elements into dst; set body, model and frame template void mjCBody::CopyList(std::vector& dst, const std::vector& src, diff --git a/src/user/user_objects.h b/src/user/user_objects.h index db4ea58f..cf4ff177 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -275,6 +275,7 @@ class mjCBody : public mjCBody_, private mjsBody { // API for adding existing objects to body mjCBody& operator+=(const mjCBody& other); + mjCBody& operator+=(const mjCFrame& other); // API for accessing objects int NumObjects(mjtObj type); @@ -360,7 +361,6 @@ class mjCFrame : public mjCFrame_, private mjsFrame { void SetParent(mjCBody* _body); mjCFrame& operator+=(const mjCBody& other); - mjCFrame& operator+=(const mjCFrame& other); private: mjCFrame(mjCModel* = 0, mjCFrame* = 0); // constructor diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 4b01e1a8..86988a5b 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -196,20 +196,18 @@ TEST_F(PluginTest, RecompileCompareCache) { } // -------------------------------- test attach ------------------------------- -TEST_F(MujocoTest, AttachSame) { - std::array er; - mjtNum tol = 0; - std::string field = ""; - - static constexpr char xml[] = R"( +static constexpr char xml_child[] = R"( - - - - - - + + + + + + + + + @@ -227,26 +225,33 @@ TEST_F(MujocoTest, AttachSame) { - + )"; +TEST_F(MujocoTest, AttachSame) { + std::array er; + mjtNum tol = 0; + std::string field = ""; + static constexpr char xml_result[] = R"( - - + + + - - + + + @@ -270,13 +275,13 @@ TEST_F(MujocoTest, AttachSame) { - - + + )"; // create parent - mjSpec* parent = ParseSpecFromString(xml, er.data(), er.size()); + mjSpec* parent = ParseSpecFromString(xml_child, er.data(), er.size()); EXPECT_THAT(parent, NotNull()) << er.data(); // get frame @@ -296,10 +301,10 @@ TEST_F(MujocoTest, AttachSame) { EXPECT_THAT(m_attached, NotNull()); // check full name stored in mjModel - EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 4), "attached-body-1"); + EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 5), "attached-body-1"); // check body 3 is attached to the world - EXPECT_THAT(m_attached->body_parentid[3], 0); + EXPECT_THAT(m_attached->body_parentid[4], 0); // compare with expected XML mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); @@ -330,32 +335,6 @@ TEST_F(MujocoTest, AttachDifferent) { )"; - static constexpr char xml_child[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - )"; - static constexpr char xml_result[] = R"( @@ -366,14 +345,15 @@ TEST_F(MujocoTest, AttachDifferent) { - + + - + @@ -381,9 +361,12 @@ TEST_F(MujocoTest, AttachDifferent) { - - + + + + + )"; // model with one free sphere and a frame @@ -403,7 +386,7 @@ TEST_F(MujocoTest, AttachDifferent) { EXPECT_THAT(body, NotNull()); // attach child to parent frame - EXPECT_THAT( + EXPECT_EQ( mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0); // compile new model @@ -430,5 +413,100 @@ TEST_F(MujocoTest, AttachDifferent) { mj_deleteModel(m_expected); } +TEST_F(MujocoTest, AttachFrame) { + std::array er; + mjtNum tol = 0; + std::string field = ""; + + static constexpr char xml_parent[] = R"( + + + + + + + + + )"; + + static constexpr char xml_result[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + // model with one free sphere and a frame + mjSpec* parent = ParseSpecFromString(xml_parent, er.data(), er.size()); + EXPECT_THAT(parent, NotNull()) << er.data(); + + // get frame + mjsBody* body = mjs_findBody(parent, "sphere"); + EXPECT_THAT(body, NotNull()); + + // model with one cylinder and a hinge + mjSpec* child = ParseSpecFromString(xml_child, er.data(), er.size()); + EXPECT_THAT(child, NotNull()) << er.data(); + + // get subtree + mjsFrame* frame = mjs_findFrame(child, "pframe"); + EXPECT_THAT(frame, NotNull()); + + // attach child to parent frame + EXPECT_THAT( + mjs_attachFrame(body, frame, /*prefix=*/"attached-", /*suffix=*/"-1"), 0); + + // compile new model + mjModel* m_attached = mjs_compile(parent, 0); + EXPECT_THAT(m_attached, NotNull()); + + // check full name stored in mjModel + EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1"); + + // check body 2 is attached to body 1 + EXPECT_THAT(m_attached->body_parentid[2], 1); + + // compare with expected XML + mjModel* m_expected = LoadModelFromString(xml_result, 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';; + + // destroy everything + mjs_deleteSpec(parent); + mjs_deleteSpec(child); + mj_deleteModel(m_attached); + mj_deleteModel(m_expected); +} + } // namespace } // namespace mujoco