diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 7ca8f985..627b6871 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4448,7 +4448,7 @@ Set element's default. .. mujoco-include:: mjs_setFrame -Set element's enclosing frame. +Set element's enclosing frame, return 0 on success. .. _mjs_resolveOrientation: diff --git a/doc/includes/references.h b/doc/includes/references.h index 355b5183..541dab8d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3683,7 +3683,7 @@ void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes); const char* mjs_getString(const mjString* source); const double* mjs_getDouble(const mjDoubleVec* source, int* size); void mjs_setDefault(mjsElement* element, const mjsDefault* def); -void mjs_setFrame(mjsElement* dest, mjsFrame* frame); +int mjs_setFrame(mjsElement* dest, mjsFrame* frame); const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); mjsFrame* mjs_bodyToFrame(mjsBody** body); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 5158e1f8..84496910 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1640,8 +1640,8 @@ MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size); // Set element's default. MJAPI void mjs_setDefault(mjsElement* element, const mjsDefault* def); -// Set element's enclosing frame. -MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); +// Set element's enclosing frame, return 0 on success. +MJAPI int mjs_setFrame(mjsElement* dest, mjsFrame* frame); // Resolve alternative orientations to quat, return error if any. MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 737de191..d2955e80 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -10423,7 +10423,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ('mjs_setFrame', FunctionDecl( name='mjs_setFrame', - return_type=ValueType(name='void'), + return_type=ValueType(name='int'), parameters=( FunctionParameterDecl( name='dest', @@ -10438,7 +10438,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc="Set element's enclosing frame.", + doc="Set element's enclosing frame, return 0 on success.", )), ('mjs_resolveOrientation', FunctionDecl( diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index bc9e5299..3a834ff3 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -564,7 +564,9 @@ PYBIND11_MODULE(_specs, m) { if (!attached_frame) { throw pybind11::value_error(mjs_getError(self.ptr)); } - mjs_setFrame(attached_frame->element, frame_ptr); + if (mjs_setFrame(attached_frame->element, frame_ptr) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } } if (site.has_value()) { raw::MjsSite* site_ptr = nullptr; @@ -637,10 +639,11 @@ PYBIND11_MODULE(_specs, m) { return out; }, py::return_value_policy::reference_internal); - mjsBody.def("set_frame", - [](raw::MjsBody& self, raw::MjsFrame& frame) -> void { - mjs_setFrame(self.element, &frame); - }); + mjsBody.def("set_frame", [](raw::MjsBody& self, raw::MjsFrame& frame) { + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } + }); mjsBody.def_property( "classname", [](raw::MjsBody& self) -> raw::MjsDefault* { @@ -850,7 +853,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSFRAME ==================================== mjsFrame.def("delete", [](raw::MjsFrame& self) { mjs_delete(self.element); }); mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { - mjs_setFrame(self.element, &frame); + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } }); mjsFrame.def_property_readonly( "parent", @@ -879,7 +884,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSGEOM ===================================== mjsGeom.def("delete", [](raw::MjsGeom& self) { mjs_delete(self.element); }); mjsGeom.def("set_frame", [](raw::MjsGeom& self, raw::MjsFrame& frame) { - mjs_setFrame(self.element, &frame); + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } }); mjsGeom.def_property_readonly( "parent", @@ -899,7 +906,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSJOINT ==================================== mjsJoint.def("delete", [](raw::MjsJoint& self) { mjs_delete(self.element); }); mjsJoint.def("set_frame", [](raw::MjsJoint& self, raw::MjsFrame& frame) { - mjs_setFrame(self.element, &frame); + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } }); mjsJoint.def_property_readonly( "parent", @@ -919,7 +928,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSSITE ===================================== mjsSite.def("delete", [](raw::MjsSite& self) { mjs_delete(self.element); }); mjsSite.def("set_frame", [](raw::MjsSite& self, raw::MjsFrame& frame) { - mjs_setFrame(self.element, &frame); + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } }); mjsSite.def_property_readonly( "parent", @@ -957,7 +968,9 @@ PYBIND11_MODULE(_specs, m) { mjsCamera.def("delete", [](raw::MjsCamera& self) { mjs_delete(self.element); }); mjsCamera.def("set_frame", [](raw::MjsCamera& self, raw::MjsFrame& frame) { - mjs_setFrame(self.element, &frame); + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } }); mjsCamera.def_property_readonly( "parent", @@ -977,7 +990,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSLIGHT ==================================== mjsLight.def("delete", [](raw::MjsLight& self) { mjs_delete(self.element); }); mjsLight.def("set_frame", [](raw::MjsLight& self, raw::MjsFrame& frame) { - mjs_setFrame(self.element, &frame); + if (mjs_setFrame(self.element, &frame) != 0) { + throw pybind11::value_error(mjs_getError(mjs_getSpec(self.element))); + } }); mjsLight.def_property_readonly( "parent", diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 0e339780..646bb986 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -758,13 +758,19 @@ mjsFrame* mjs_findFrame(mjSpec* s, const char* name) { // set frame -void mjs_setFrame(mjsElement* dest, mjsFrame* frame) { +int mjs_setFrame(mjsElement* dest, mjsFrame* frame) { if (!frame) { - return; + return -1; } mjCFrame* frameC = static_cast(frame->element); mjCBase* baseC = static_cast(dest); - baseC->SetFrame(frameC); + try { + baseC->SetFrame(frameC); + return 0; + } catch (mjCError& e) { + baseC->model->SetError(e); + return -1; + } } diff --git a/src/user/user_api.h b/src/user/user_api.h index 3c6c4420..01aa9856 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -370,8 +370,8 @@ MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size); // Set element's default. MJAPI void mjs_setDefault(mjsElement* element, const mjsDefault* def); -// Set element's enlcosing frame. -MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); +// Set element's enclosing frame, return 0 on success. +MJAPI int mjs_setFrame(mjsElement* dest, mjsFrame* frame); // Resolve alternative orientations to quat, return error if any. MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 1e9cf407..d65ac68b 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -773,6 +773,9 @@ void mjCBase::SetFrame(mjCFrame* _frame) { if (!_frame) { return; } + if (_frame->body && GetParent() != _frame->body) { + throw mjCError(this, "Frame and body '%s' have mismatched parents", name.c_str()); + } frame = _frame; } @@ -891,8 +894,18 @@ mjCBody& mjCBody::operator+=(const mjCBody& other) { for (int i=0; i < other.bodies.size(); i++) { bodies.push_back(new mjCBody(*other.bodies[i], model)); // triggers recursive call bodies.back()->parent = this; - bodies.back()->frame = - other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : nullptr; + bodies.back()->frame = nullptr; + if (other.bodies[i]->frame) { + if (fmap.find(other.bodies[i]->frame) != fmap.end()) { + bodies.back()->frame = frames[fmap[other.bodies[i]->frame]]; + } else { + throw mjCError(this, "Frame '%s' not found in other body", + other.bodies[i]->frame->name.c_str()); + } + if (bodies.back()->frame && bodies.back()->frame->body != this) { + throw mjCError(this, "Frame and body '%s' have mismatched parents", name.c_str()); + } + } } return *this; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 6d9b146e..f04bf850 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -254,6 +254,9 @@ class mjCBase : public mjCBase_ { // Copy plugins instantiated in this object virtual void CopyPlugin() {} + // Returns parent of this object + virtual mjCBase* GetParent() const { return nullptr; } + // Copy assignment mjCBase& operator=(const mjCBase& other); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 272b5a2c..7fda10cd 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -3647,7 +3647,9 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, } } else { // only set frame to existing body - mjs_setFrame(child->element, pframe); + if (mjs_setFrame(child->element, pframe)) { + throw mjXError(elem, mjs_getError(spec)); + } } } diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 52650330..7b74af4c 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1449,6 +1449,67 @@ TEST_F(XMLReaderTest, ParseReplicateRepeatedName) { EXPECT_THAT(error.data(), HasSubstr("Element 'replicate'")); } +TEST_F(XMLReaderTest, RepeatedPrefix) { + static constexpr char parent[] = R"( + + + + + + + + + + + + + + )"; + + static constexpr char child_1[] = R"( + + + + + + + + + + + + + + )"; + + static constexpr char child_2[] = R"( + + + + + + )"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "child_1.xml", child_1, sizeof(child_1)); + mj_addBufferVFS(vfs.get(), "child_2.xml", child_2, sizeof(child_2)); + + std::array err; + mjSpec* c2 = mj_parseXMLString(child_2, 0, err.data(), err.size()); + EXPECT_THAT(c2, NotNull()) << err.data(); + mjSpec* c1 = mj_parseXMLString(child_1, vfs.get(), err.data(), err.size()); + EXPECT_THAT(c1, NotNull()) << err.data(); + mj_deleteSpec(c1); + mj_deleteSpec(c2); + + mjSpec* spec = mj_parseXMLString(parent, vfs.get(), err.data(), err.size()); + EXPECT_THAT(spec, IsNull()); + EXPECT_THAT(err.data(), HasSubstr("mismatched parents")); + mj_deleteSpec(spec); + mj_deleteVFS(vfs.get()); +} + TEST_F(XMLReaderTest, ParseReplicateExcludeTendon) { static constexpr char xml[] = R"(