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

Add mjs_attachFrameToSite.

Browse Source 
This is required in preparation of changing the spec.attach behavior in the Python bindings.

PiperOrigin-RevId: 730810198
Change-Id: I095808f0afe62b5bbd27201ed5c75804e0e9c4bf
This commit is contained in:
Alessio Quaglino
2025-02-25 03:20:06 -08:00
committed by Copybara-Service
parent 6b11f2b88e
commit ce82b63155
7 changed files with 147 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+9
View File
@@ -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>`__
doc/includes/references.h
+2
View File
@@ -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);
include/mujoco/mujoco.h
+4
View File
@@ -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);
python/mujoco/introspect/functions.py
+34
View File
@@ -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',
src/user/user_api.cc
+29
View File
@@ -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<mjCSite*>(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<mjCModel*>(s->element);
src/user/user_api.h
+4
View File
@@ -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);
test/user/user_api_test.cc
+65
View File
@@ -1367,6 +1367,71 @@ TEST_F(MujocoTest, AttachToSite) {
mj_deleteModel(expected);
}
TEST_F(MujocoTest, AttachFrameToSite) {
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>
<frame name="frame">
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
</frame>
</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());
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<char, 1000> er;
mjtNum tol = 0;