diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 56bc272e..290447f6 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3813,6 +3813,15 @@ mjs_attachFrame Attach child frame to a parent body, return the attached frame if success or NULL otherwise. +.. _mjs_attachToSite: + +mjs_attachToSite +~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_attachToSite + +Attach child body to a parent site, return the attached body if success or NULL otherwise. + .. _mjs_detachBody: mjs_detachBody @@ -4126,15 +4135,6 @@ mjs_getSpec Get spec from body. -.. _mjs_getSpecFromFrame: - -mjs_getSpecFromFrame -~~~~~~~~~~~~~~~~~~~~ - -.. mujoco-include:: mjs_getSpecFromFrame - -Get spec from frame. - .. _mjs_findBody: mjs_findBody diff --git a/doc/includes/references.h b/doc/includes/references.h index 6e193719..e65e2412 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3564,6 +3564,8 @@ mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child, const char* prefix, const char* suffix); mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, const char* prefix, const char* suffix); +mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, + const char* prefix, const char* suffix); int mjs_detachBody(mjSpec* s, mjsBody* b); mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def); mjsSite* mjs_addSite(mjsBody* body, mjsDefault* def); @@ -3596,8 +3598,7 @@ mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); mjsTexture* mjs_addTexture(mjSpec* s); mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); -mjSpec* mjs_getSpec(mjsBody* body); -mjSpec* mjs_getSpecFromFrame(mjsFrame* frame); +mjSpec* mjs_getSpec(mjsElement* element); mjsBody* mjs_findBody(mjSpec* s, const char* name); mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); mjsBody* mjs_findChild(mjsBody* body, const char* name); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index c146054c..ebcd5b6c 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1415,6 +1415,10 @@ MJAPI mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child, MJAPI mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, const char* prefix, const char* suffix); +// Attach child body to a parent site, return the attached body if success or NULL otherwise. +MJAPI mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, + const char* prefix, const char* suffix); + // Detach body from mjSpec, remove all references and delete the body, return 0 on success. MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); @@ -1524,10 +1528,7 @@ MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); //---------------------------------- Find and get utilities ---------------------------------------- // Get spec from body. -MJAPI mjSpec* mjs_getSpec(mjsBody* body); - -// Get spec from frame. -MJAPI mjSpec* mjs_getSpecFromFrame(mjsFrame* frame); +MJAPI mjSpec* mjs_getSpec(mjsElement* element); // Find body in spec by name. MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name); diff --git a/introspect/functions.py b/introspect/functions.py index 447b2e55..cea58daa 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -9030,6 +9030,40 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Attach child frame to a parent body, return the attached frame if success or NULL otherwise.', # pylint: disable=line-too-long )), + ('mjs_attachToSite', + FunctionDecl( + name='mjs_attachToSite', + return_type=PointerType( + inner_type=ValueType(name='mjsBody'), + ), + parameters=( + FunctionParameterDecl( + name='parent', + type=PointerType( + inner_type=ValueType(name='mjsSite'), + ), + ), + FunctionParameterDecl( + name='child', + type=PointerType( + inner_type=ValueType(name='mjsBody', is_const=True), + ), + ), + FunctionParameterDecl( + name='prefix', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + FunctionParameterDecl( + name='suffix', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Attach child body to a parent site, return the attached body if success or NULL otherwise.', # pylint: disable=line-too-long + )), ('mjs_detachBody', FunctionDecl( name='mjs_detachBody', @@ -9674,30 +9708,14 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), parameters=( FunctionParameterDecl( - name='body', + name='element', type=PointerType( - inner_type=ValueType(name='mjsBody'), + inner_type=ValueType(name='mjsElement'), ), ), ), doc='Get spec from body.', )), - ('mjs_getSpecFromFrame', - FunctionDecl( - name='mjs_getSpecFromFrame', - return_type=PointerType( - inner_type=ValueType(name='mjSpec'), - ), - parameters=( - FunctionParameterDecl( - name='frame', - type=PointerType( - inner_type=ValueType(name='mjsFrame'), - ), - ), - ), - doc='Get spec from frame.', - )), ('mjs_findBody', FunctionDecl( name='mjs_findBody', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index cb11562f..20596b6c 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -426,7 +426,7 @@ PYBIND11_MODULE(_specs, m) { [](raw::MjsBody& self, mjtObj objtype) -> py::list { py::list list; raw::MjsElement* el = mjs_firstChild(&self, objtype, true); - std::string error = mjs_getError(mjs_getSpec(&self)); + std::string error = mjs_getError(mjs_getSpec(self.element)); if (!el && !error.empty()) { throw pybind11::value_error(error); } @@ -557,7 +557,9 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); mjsBody.def( "spec", - [](raw::MjsBody& self) -> raw::MjSpec* { return mjs_getSpec(&self); }, + [](raw::MjsBody& self) -> raw::MjSpec* { + return mjs_getSpec(self.element); + }, py::return_value_policy::reference_internal); mjsBody.def( "attach_frame", @@ -566,7 +568,7 @@ PYBIND11_MODULE(_specs, m) { auto new_frame = mjs_attachFrame(&self, &frame, prefix.c_str(), suffix.c_str()); if (!new_frame) { - throw pybind11::value_error(mjs_getError(mjs_getSpec(&self))); + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); } return new_frame; }, @@ -587,7 +589,7 @@ PYBIND11_MODULE(_specs, m) { mjs_attachBody(&self, &body, prefix.c_str(), suffix.c_str()); if (!new_body) { throw pybind11::value_error( - mjs_getError(mjs_getSpecFromFrame(&self))); + mjs_getError(mjs_getSpec(self.element))); } return new_body; }, @@ -643,6 +645,19 @@ PYBIND11_MODULE(_specs, m) { return mjs_getDefault(self.element); }, py::return_value_policy::reference_internal); + mjsSite.def( + "attach", + [](raw::MjsSite& self, raw::MjsBody& body, std::string& prefix, + std::string& suffix) -> raw::MjsBody* { + auto new_body = + mjs_attachToSite(&self, &body, prefix.c_str(), suffix.c_str()); + if (!new_body) { + throw pybind11::value_error( + mjs_getError(mjs_getSpec(self.element))); + } + return new_body; + }, + py::return_value_policy::reference_internal); // ============================= MJSCAMERA =================================== mjsCamera.def_property_readonly("id", [](raw::MjsCamera& self) -> int { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index acce44d1..3c8618f1 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -857,6 +857,15 @@ class SpecsTest(absltest.TestCase): ' attribute', ) + def test_attach_body_to_site(self): + child = mujoco.MjSpec() + parent = mujoco.MjSpec() + site = parent.worldbody.add_site(pos=[1, 2, 3]) + body = child.worldbody.add_body() + self.assertIsNotNone(site.attach(body, '', '')) + model = parent.compile() + np.testing.assert_array_equal(model.body_pos[1], [1, 2, 3]) + if __name__ == '__main__': absltest.main() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 12b7a1d2..052e07ed 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -29,6 +29,7 @@ #include "user/user_model.h" #include "user/user_objects.h" #include "user/user_cache.h" +#include "user/user_util.h" namespace { @@ -160,6 +161,29 @@ mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, +// attach child body to a parent site +mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, + const char* prefix, const char* suffix) { + if (!parent) { + mju_error("parent site is null"); + return nullptr; + } + mjCSite* site = static_cast(parent->element); + mjCBody* body = site->Body(); + mjCFrame* frame = body->AddFrame(site->frame); + frame->spec.pos[0] = site->spec.pos[0]; + frame->spec.pos[1] = site->spec.pos[1]; + frame->spec.pos[2] = site->spec.pos[2]; + frame->spec.quat[0] = site->spec.quat[0]; + frame->spec.quat[1] = site->spec.quat[1]; + frame->spec.quat[2] = site->spec.quat[2]; + frame->spec.quat[3] = site->spec.quat[3]; + frame->SetParent(body); + return mjs_attachBody(&frame->spec, child, prefix, suffix); +} + + + // get error message from model const char* mjs_getError(mjSpec* s) { mjCModel* modelC = static_cast(s->element); @@ -521,15 +545,8 @@ mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* p // get spec from body -mjSpec* mjs_getSpec(mjsBody* body) { - return &(static_cast(body->element)->model->spec); -} - - - -// get spec from frame -mjSpec* mjs_getSpecFromFrame(mjsFrame* frame) { - return &(static_cast(frame->element)->model->spec); +mjSpec* mjs_getSpec(mjsElement* element) { + return &(static_cast(element)->model->spec); } diff --git a/src/user/user_api.h b/src/user/user_api.h index 853c9363..95f2b718 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -71,11 +71,15 @@ MJAPI int mjs_activatePlugin(mjSpec* s, const char* name); // Attach child body to a parent frame, return the attached body if success or NULL otherwise. MJAPI mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child, - const char* prefix, const char* suffix); + const char* prefix, const char* suffix); // Attach child frame to a parent body, return the attached frame if success or NULL otherwise. MJAPI mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, - const char* prefix, const char* suffix); + const char* prefix, const char* suffix); + +// Attach child body to a parent site, return the attached body if success or NULL otherwise. +MJAPI mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, + const char* prefix, const char* suffix); // Detach body from mjSpec, remove all references and delete the body, return 0 on success. MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); @@ -186,10 +190,7 @@ MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); //---------------------------------- Find/get utilities -------------------------------------------- // Get spec from body. -MJAPI mjSpec* mjs_getSpec(mjsBody* body); - -// Get spec from frame. -MJAPI mjSpec* mjs_getSpecFromFrame(mjsFrame* frame); +MJAPI mjSpec* mjs_getSpec(mjsElement* element); // Find spec (model asset) by name. MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 82641520..2ccf8bd9 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -587,6 +587,9 @@ class mjCSite : public mjCSite_, private mjsSite { mjsSite spec; // variables set by user + // site's body + mjCBody* Body() const { return body; } + // use strings from mjCBase rather than mjStrings from mjsSite using mjCBase::name; using mjCBase::info; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index cd1ebeb7..7c06c6e3 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1079,6 +1079,69 @@ TEST_F(MujocoTest, DetachBody) { TestDetachBody(/*compile=*/true); } +TEST_F(MujocoTest, AttachToSite) { + std::array er; + mjtNum tol = 0; + std::string field = ""; + + static constexpr char xml_parent[] = R"( + + + + + )"; + + static constexpr char xml_child[] = R"( + + + + + + + + )"; + + static constexpr char xml_result[] = R"( + + + + + + + + + + + )"; + + 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 er; std::string field = "";