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

Do not overwrite existing parent frames for worldbody elements when attaching an mjSpec to a frame or a site.

Browse Source 
PiperOrigin-RevId: 737662046
Change-Id: I7028c21c803c01845ae1d53cb476f7b3df172313
This commit is contained in:
Alessio Quaglino
2025-03-17 10:20:42 -07:00
committed by Copybara-Service
parent da04688071
commit 1bf24e9f67
9 changed files with 59 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
python/mujoco/introspect/functions.py
+16
View File
@@ -9896,6 +9896,22 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Get parent body.',
)),
('mjs_getFrame',
FunctionDecl(
name='mjs_getFrame',
return_type=PointerType(
inner_type=ValueType(name='mjsFrame'),
),
parameters=(
FunctionParameterDecl(
name='element',
type=PointerType(
inner_type=ValueType(name='mjsElement'),
),
),
),
doc='Get parent frame.',
)),
('mjs_findFrame',
FunctionDecl(
name='mjs_findFrame',
python/mujoco/specs.cc
+1 -1
View File
@@ -272,7 +272,7 @@ py::list FindAllImpl(raw::MjsBody& body, mjtObj objtype, bool recursive) {
void SetFrame(raw::MjsBody* body, mjtObj objtype, raw::MjsFrame* frame) {
mjsElement* el = mjs_firstChild(body, objtype, 0);
while (el) {
if (frame->element != el) {
if (frame->element != el && mjs_getFrame(el) == nullptr) {
mjs_setFrame(el, frame);
}
el = mjs_nextChild(body, el, 0);
python/mujoco/specs_test.py
+5 -3
View File
@@ -1072,6 +1072,7 @@ class SpecsTest(absltest.TestCase):
child2 = mujoco.MjSpec()
child2.assets = {'cube2.obj': 'cube2_content'}
body2 = child2.worldbody.add_body(name='body')
body2.set_frame(child2.worldbody.add_frame(pos=[-1, -1, 1]))
self.assertIsNotNone(parent.attach(child2, frame=frame, prefix='child-'))
self.assertIsNotNone(child2.worldbody)
self.assertEqual(child2.parent, parent)
@@ -1080,7 +1081,7 @@ class SpecsTest(absltest.TestCase):
self.assertIsNotNone(model2)
self.assertEqual(model2.nbody, 3)
np.testing.assert_array_equal(model2.body_pos[1], [0, 1, 4])
np.testing.assert_array_equal(model2.body_pos[2], [2, 3, 2])
np.testing.assert_array_equal(model2.body_pos[2], [3, 4, 3])
np.testing.assert_array_equal(model2.body_quat[1], [0, 0, 0, 1])
np.testing.assert_array_equal(model2.body_quat[2], [0, 0, 0, 1])
self.assertEqual(parent.assets['cube.obj'], 'cube_content')
@@ -1090,6 +1091,7 @@ class SpecsTest(absltest.TestCase):
child3 = mujoco.MjSpec()
child3.assets = {'cube2.obj': 'new_cube2_content'}
body3 = child3.worldbody.add_body(name='body')
body3.set_frame(child3.worldbody.add_frame(pos=[-1, -1, 1]))
self.assertIsNotNone(parent.attach(child3, frame='frame', prefix='child3-'))
self.assertIsNotNone(child3.worldbody)
self.assertEqual(child3.parent, parent)
@@ -1098,8 +1100,8 @@ class SpecsTest(absltest.TestCase):
self.assertIsNotNone(model3)
self.assertEqual(model3.nbody, 4)
np.testing.assert_array_equal(model3.body_pos[1], [0, 1, 4])
np.testing.assert_array_equal(model3.body_pos[2], [2, 3, 2])
np.testing.assert_array_equal(model3.body_pos[3], [3, 4, 1])
np.testing.assert_array_equal(model3.body_pos[2], [3, 4, 3])
np.testing.assert_array_equal(model3.body_pos[3], [4, 5, 2])
np.testing.assert_array_equal(model3.body_quat[1], [0, 0, 0, 1])
np.testing.assert_array_equal(model3.body_quat[2], [0, 0, 0, 1])
np.testing.assert_array_equal(model3.body_quat[3], [0, 0, 0, 1])