diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 30f62c87..1d2bd252 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3793,7 +3793,7 @@ mjs_attachBody .. mujoco-include:: mjs_attachBody -Attach child body to a parent frame, return 0 on success. +Attach child body to a parent frame, return the attached body if success or NULL otherwise. .. _mjs_attachFrame: @@ -3802,7 +3802,7 @@ mjs_attachFrame .. mujoco-include:: mjs_attachFrame -Attach child frame to a parent body, return 0 on success. +Attach child frame to a parent body, return the attached frame if success or NULL otherwise. .. _mjs_detachBody: diff --git a/doc/includes/references.h b/doc/includes/references.h index a1d4c405..c3655ac9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3550,10 +3550,10 @@ void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task); void mju_threadPoolDestroy(mjThreadPool* thread_pool); void mju_defaultTask(mjTask* task); void mju_taskJoin(mjTask* task); -int mjs_attachBody(mjsFrame* parent, const mjsBody* child, - const char* prefix, const char* suffix); -int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, - const char* prefix, const char* suffix); +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); int mjs_detachBody(mjSpec* s, mjsBody* b); mjsBody* mjs_addBody(mjsBody* body, mjsDefault* def); mjsSite* mjs_addSite(mjsBody* body, mjsDefault* def); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 3475a682..abdfa5cc 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1404,13 +1404,13 @@ MJAPI void mju_taskJoin(mjTask* task); //---------------------------------- Attachment ---------------------------------------------------- -// Attach child body to a parent frame, return 0 on success. -MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child, - const char* prefix, const char* suffix); +// 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); -// Attach child frame to a parent body, return 0 on success. -MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, - 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); // Detach body from mjSpec, remove all references and delete the body, return 0 on success. MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); diff --git a/introspect/functions.py b/introspect/functions.py index 98dce5fd..6d796d9e 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -8945,7 +8945,9 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ('mjs_attachBody', FunctionDecl( name='mjs_attachBody', - return_type=ValueType(name='int'), + return_type=PointerType( + inner_type=ValueType(name='mjsBody'), + ), parameters=( FunctionParameterDecl( name='parent', @@ -8972,12 +8974,14 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Attach child body to a parent frame, return 0 on success.', + doc='Attach child body to a parent frame, return the attached body if success or NULL otherwise.', # pylint: disable=line-too-long )), ('mjs_attachFrame', FunctionDecl( name='mjs_attachFrame', - return_type=ValueType(name='int'), + return_type=PointerType( + inner_type=ValueType(name='mjsFrame'), + ), parameters=( FunctionParameterDecl( name='parent', @@ -9004,7 +9008,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Attach child frame to a parent body, return 0 on success.', + doc='Attach child frame to a parent body, return the attached frame if success or NULL otherwise.', # pylint: disable=line-too-long )), ('mjs_detachBody', FunctionDecl( diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index c3c37d5b..e807a8c9 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -683,11 +683,13 @@ PYBIND11_MODULE(_specs, m) { "spec", [](raw::MjsBody& self) -> raw::MjSpec* { return mjs_getSpec(&self); }, py::return_value_policy::reference_internal); - mjsBody.def("attach_frame", - [](raw::MjsBody& self, raw::MjsFrame& frame, std::string& prefix, - std::string& suffix) -> void { - mjs_attachFrame(&self, &frame, prefix.c_str(), suffix.c_str()); - }); + mjsBody.def( + "attach_frame", + [](raw::MjsBody& self, raw::MjsFrame& frame, std::string& prefix, + std::string& suffix) -> raw::MjsFrame* { + return mjs_attachFrame(&self, &frame, prefix.c_str(), suffix.c_str()); + }, + py::return_value_policy::reference_internal); // ============================= MJSFRAME ==================================== mjsFrame.def_property_readonly( @@ -696,10 +698,13 @@ PYBIND11_MODULE(_specs, m) { mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { mjs_setFrame(self.element, &frame); }); - mjsFrame.def("attach_body", [](raw::MjsFrame& self, raw::MjsBody& body, - std::string& prefix, std::string& suffix) { - mjs_attachBody(&self, &body, prefix.c_str(), suffix.c_str()); - }); + mjsFrame.def( + "attach_body", + [](raw::MjsFrame& self, raw::MjsBody& body, std::string& prefix, + std::string& suffix) -> raw::MjsBody* { + return mjs_attachBody(&self, &body, prefix.c_str(), suffix.c_str()); + }, + py::return_value_policy::reference_internal); // ============================= MJSGEOM ===================================== mjsGeom.def_property_readonly( diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 92e1e8c6..739cc863 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -117,33 +117,37 @@ mjModel* mj_compile(mjSpec* s, const mjVFS* vfs) { // attach body to a frame of the parent -int mjs_attachBody(mjsFrame* parent, const mjsBody* child, - const char* prefix, const char* suffix) { +mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child, + const char* prefix, const char* suffix) { mjCFrame* frame_parent = static_cast(parent->element); mjCBody* child_body = static_cast(child->element); try { *frame_parent += std::string(prefix) + *child_body + std::string(suffix); } catch (mjCError& e) { frame_parent->model->SetError(e); - return -1; + return nullptr; } - return 0; + mjsBody* attached_body = frame_parent->last_attached; + frame_parent->last_attached = nullptr; + return attached_body; } // attach frame to a parent body -int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, - const char* prefix, const char* suffix) { +mjsFrame* 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); try { *body_parent += std::string(prefix) + *child_frame + std::string(suffix); } catch (mjCError& e) { body_parent->model->SetError(e); - return -1; + return nullptr; } - return 0; + mjsFrame* attached_frame = body_parent->last_attached; + body_parent->last_attached = nullptr; + return attached_frame; } diff --git a/src/user/user_api.h b/src/user/user_api.h index a1b80dd2..c2855b54 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -66,12 +66,12 @@ MJAPI void mjs_addSpec(mjSpec* s, mjSpec* child); //---------------------------------- Attachment ---------------------------------------------------- -// Attach child body to a parent frame, return 0 on success. -MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child, +// 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); -// Attach child frame to a parent body, return 0 on success. -MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, +// 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); // Detach body from mjSpec, remove all references and delete the body, return 0 on success. diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 7d369998..a5da1d58 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -762,6 +762,7 @@ mjCBody::mjCBody(mjCModel* _model) { margin = 0; mjuu_zerovec(xpos0, 3); mjuu_setvec(xquat0, 1, 0, 0, 0); + last_attached = nullptr; // clear object lists bodies.clear(); @@ -860,7 +861,9 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { frames.back()->body = this; frames.back()->model = model; frames.back()->frame = other.frame; + frames.back()->NameSpace(other.model); int i = frames.size(); + last_attached = &frames.back()->spec; // map input frames to index in this->frames std::map fmap; @@ -1723,6 +1726,7 @@ mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) { model = _model; body = NULL; frame = _frame ? _frame : NULL; + last_attached = nullptr; PointToLocal(); CopyFromSpec(); } @@ -1771,6 +1775,7 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { // add to body children body->bodies.push_back(subtree); + last_attached = &body->bodies.back()->spec; // attach referencing elements *model += *other.model; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5a763e43..ea5b6f71 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -331,6 +331,8 @@ class mjCBody : public mjCBody_, private mjsBody { mjtNum* mpos(const std::string& state_name); mjtNum* mquat(const std::string& state_name); + mjsFrame* last_attached; // last attached frame to this body + private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor mjCBody& operator=(const mjCBody& other); // copy assignment @@ -398,6 +400,8 @@ class mjCFrame : public mjCFrame_, private mjsFrame { bool IsAncestor(const mjCFrame* child) const; // true if child is contained in this frame + mjsBody* last_attached; // last attached body to this frame + private: void Compile(void); // compiler diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 04fe3125..c6587c2c 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -3627,7 +3627,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, UpdateString(suffix, count, i); // attach to parent - if (mjs_attachFrame(body, pframe, /*prefix=*/"", suffix.c_str()) != 0) { + if (!mjs_attachFrame(body, pframe, /*prefix=*/"", suffix.c_str())) { throw mjXError(elem, mjs_getError(spec)); } } @@ -3704,7 +3704,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, if (!child) { throw mjXError(0, "could not find body '%s''%s'", body_name.c_str()); } - if (mjs_attachBody(pframe, child, prefix.c_str(), "") != 0) { + if (!mjs_attachBody(pframe, child, prefix.c_str(), "")) { throw mjXError(elem, mjs_getError(spec)); } } else { diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index e5e29dff..0548a15d 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -660,8 +660,8 @@ TEST_F(MujocoTest, AttachSame) { EXPECT_THAT(body, NotNull()); // attach child to parent frame - EXPECT_THAT( - mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0); + mjsBody* attached = mjs_attachBody(frame, body, "attached-", "-1"); + EXPECT_THAT(attached, mjs_findBody(parent, "attached-body-1")); // compile new model mjModel* m_attached = mj_compile(parent, 0); @@ -782,8 +782,8 @@ TEST_F(MujocoTest, AttachDifferent) { EXPECT_THAT(body, NotNull()); // attach child to parent frame - EXPECT_EQ( - mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0); + mjsBody* attached = mjs_attachBody(frame, body, "attached-", "-1"); + EXPECT_THAT(attached, mjs_findBody(parent, "attached-body-1")); // compile new model mjModel* m_attached = mj_compile(parent, 0); @@ -904,8 +904,8 @@ TEST_F(MujocoTest, AttachFrame) { EXPECT_THAT(frame, NotNull()); // attach child frame to parent body - EXPECT_THAT( - mjs_attachFrame(body, frame, /*prefix=*/"attached-", /*suffix=*/"-1"), 0); + mjsFrame* attached = mjs_attachFrame(body, frame, "attached-", "-1"); + EXPECT_THAT(attached, mjs_findFrame(parent, "attached-pframe-1")); // compile new model mjModel* m_attached = mj_compile(parent, 0);