diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 64c50073..feea786f 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3836,6 +3836,15 @@ Attach child frame to a parent body, return the attached frame if success or NUL Attach child body to a parent site, return the attached body if success or NULL otherwise. +.. _mjs_attachFrameToSite: + +`mjs_attachFrameToSite <#mjs_attachFrameToSite>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_attachFrameToSite + +Attach child frame to a parent site, return the attached frame if success or NULL otherwise. + .. _mjs_detachBody: `mjs_detachBody <#mjs_detachBody>`__ diff --git a/doc/includes/references.h b/doc/includes/references.h index e411bf2e..7609d25e 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3602,6 +3602,8 @@ 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); +mjsFrame* mjs_attachFrameToSite(mjsSite* parent, const mjsFrame* child, + const char* prefix, const char* suffix); int mjs_detachBody(mjSpec* s, mjsBody* b); mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 98f559cf..7343b8ff 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1426,6 +1426,10 @@ MJAPI mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, MJAPI mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, const char* prefix, const char* suffix); +// Attach child frame to a parent site, return the attached frame if success or NULL otherwise. +MJAPI mjsFrame* mjs_attachFrameToSite(mjsSite* 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/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 730c03f1..ab1fc701 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -9102,6 +9102,40 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Attach child body to a parent site, return the attached body if success or NULL otherwise.', # pylint: disable=line-too-long )), + ('mjs_attachFrameToSite', + FunctionDecl( + name='mjs_attachFrameToSite', + return_type=PointerType( + inner_type=ValueType(name='mjsFrame'), + ), + parameters=( + FunctionParameterDecl( + name='parent', + type=PointerType( + inner_type=ValueType(name='mjsSite'), + ), + ), + FunctionParameterDecl( + name='child', + type=PointerType( + inner_type=ValueType(name='mjsFrame', 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 frame to a parent site, return the attached frame if success or NULL otherwise.', # pylint: disable=line-too-long + )), ('mjs_detachBody', FunctionDecl( name='mjs_detachBody', diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 5c9bb28f..2085e207 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -190,6 +190,35 @@ mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, +// attach child frame to a parent site +mjsFrame* mjs_attachFrameToSite(mjsSite* parent, const mjsFrame* child, + const char* prefix, const char* suffix) { + if (!parent) { + mju_error("parent site is null"); + return nullptr; + } + mjSpec* spec = mjs_getSpec(parent->element); + mjCSite* site = static_cast(parent->element); + mjCBody* body = site->Body(); + mjCFrame* frame = body->AddFrame(site->frame); + frame->SetParent(body); + 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]; + mjs_resolveOrientation(frame->spec.quat, spec->compiler.degree, + spec->compiler.eulerseq, &site->spec.alt); + + mjsFrame* attached_frame = mjs_attachFrame(&body->spec, child, prefix, suffix); + mjs_setFrame(attached_frame->element, &frame->spec); + return attached_frame; +} + + + // 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 89079048..be16d115 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -81,6 +81,10 @@ MJAPI mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child, MJAPI mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, const char* prefix, const char* suffix); +// Attach child frame to a parent site, return the attached frame if success or NULL otherwise. +MJAPI mjsFrame* mjs_attachFrameToSite(mjsSite* 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/test/user/user_api_test.cc b/test/user/user_api_test.cc index b0e5e6b4..899232d3 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -1367,6 +1367,71 @@ TEST_F(MujocoTest, AttachToSite) { mj_deleteModel(expected); } +TEST_F(MujocoTest, AttachFrameToSite) { + 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()); + mjsFrame* frame = mjs_findFrame(child, "frame"); + EXPECT_THAT(frame, NotNull()); + mjsFrame* attached = mjs_attachFrameToSite(site, frame, "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, AttachWorld) { std::array er; mjtNum tol = 0;