From be64747166a1b61dfb26f80b616890296bcd347f Mon Sep 17 00:00:00 2001 From: Tom Power Date: Wed, 19 Mar 2025 10:30:08 -0700 Subject: [PATCH] add `mjs_detachDefault` to delete a default class from `mjSpec` PiperOrigin-RevId: 738452369 Change-Id: I5a53a6dc8f89e1e8136aeefc09d7d9b7b7204e1f --- doc/APIreference/functions.rst | 14 +++- doc/APIreference/functions_override.rst | 5 ++ doc/includes/references.h | 7 +- include/mujoco/mjmodel.h | 4 +- include/mujoco/mujoco.h | 6 +- python/mujoco/introspect/enums.py | 1 + python/mujoco/introspect/functions.py | 24 ++++++- python/mujoco/specs.cc | 7 +- python/mujoco/specs_test.py | 6 ++ src/engine/engine_io.c | 1 + src/user/user_api.cc | 33 ++++++++-- src/user/user_api.h | 4 +- src/user/user_model.cc | 62 +++++++++++++++++- src/user/user_model.h | 12 ++-- src/user/user_objects.cc | 11 ++++ src/user/user_objects.h | 2 + test/user/user_api_test.cc | 85 +++++++++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 1 + 18 files changed, 261 insertions(+), 24 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index f0856fee..7ca8f985 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3851,7 +3851,16 @@ Attach child frame to a parent site, return the attached frame if success or NUL .. mujoco-include:: mjs_detachBody -Detach body from mjSpec, remove all references and delete the body, return 0 on success. +Delete body and descendants from mjSpec, remove all references, return 0 on success. + +.. _mjs_detachDefault: + +`mjs_detachDefault <#mjs_detachDefault>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_detachDefault + +Delete default class and descendants from mjSpec, remove all references, return 0 on success. .. _AddTreeElements: @@ -3936,7 +3945,8 @@ Add frame to body. .. mujoco-include:: mjs_delete -Delete object corresponding to the given element, return 0 on success. +Delete object corresponding to the given element, return 0 on success. This function should only be used for element +types that cannot have children, i.e. excluding bodies and default classes. .. _AddNonTreeElements: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index afd18be4..ec72a183 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -704,3 +704,8 @@ to the inputs. Below, :math:`\bar q` denotes the pre-modified quaternion: Note that derivatives depend only on :math:`h` and :math:`v` (in fact, on :math:`s = h v`). All outputs are optional. + +.. _mjs_delete: + +Delete object corresponding to the given element, return 0 on success. This function should only be used for element +types that cannot have children, i.e. excluding bodies and default classes. diff --git a/doc/includes/references.h b/doc/includes/references.h index d2bf484e..39eece88 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -603,7 +603,9 @@ typedef enum mjtObj_ { // type of MujoCo object mjNOBJECT, // number of object types // meta elements, do not appear in mjModel - mjOBJ_FRAME = 100 // frame + mjOBJ_FRAME = 100, // frame + mjOBJ_DEFAULT // default + } mjtObj; typedef enum mjtConstraint_ { // type of constraint mjCNSTR_EQUALITY = 0, // equality constraint @@ -3612,6 +3614,7 @@ mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, mjsFrame* mjs_attachFrameToSite(mjsSite* parent, const mjsFrame* child, const char* prefix, const char* suffix); int mjs_detachBody(mjSpec* s, mjsBody* b); +int mjs_detachDefault(mjSpec* s, mjsDefault* d); mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); mjsJoint* mjs_addJoint(mjsBody* body, const mjsDefault* def); @@ -3652,7 +3655,7 @@ mjsBody* mjs_getParent(mjsElement* element); mjsFrame* mjs_getFrame(mjsElement* element); mjsFrame* mjs_findFrame(mjSpec* s, const char* name); mjsDefault* mjs_getDefault(mjsElement* element); -const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); mjsDefault* mjs_getSpecDefault(mjSpec* s); int mjs_getId(mjsElement* element); mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse); diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 5fddbec0..0099cf78 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -268,7 +268,9 @@ typedef enum mjtObj_ { // type of MujoCo object mjNOBJECT, // number of object types // meta elements, do not appear in mjModel - mjOBJ_FRAME = 100 // frame + mjOBJ_FRAME = 100, // frame + mjOBJ_DEFAULT // default + } mjtObj; diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index dd470087..5158e1f8 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1430,9 +1430,11 @@ MJAPI mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child, 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. +// Delete body and descendants from mjSpec, remove all references, return 0 on success. MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); +// Delete default class and descendants from mjSpec, remove all references, return 0 on success. +MJAPI int mjs_detachDefault(mjSpec* s, mjsDefault* d); //---------------------------------- Tree elements ------------------------------------------------- @@ -1566,7 +1568,7 @@ MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); MJAPI mjsDefault* mjs_getDefault(mjsElement* element); // Find default in model by class name. -MJAPI const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); // Get global default from model. MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 2b35d71d..66b937a2 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -287,6 +287,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjOBJ_PLUGIN', 25), ('mjNOBJECT', 26), ('mjOBJ_FRAME', 100), + ('mjOBJ_DEFAULT', 101), ]), )), ('mjtConstraint', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 62fef7cb..737de191 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -9154,7 +9154,27 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Detach body from mjSpec, remove all references and delete the body, return 0 on success.', # pylint: disable=line-too-long + doc='Delete body and descendants from mjSpec, remove all references, return 0 on success.', # pylint: disable=line-too-long + )), + ('mjs_detachDefault', + FunctionDecl( + name='mjs_detachDefault', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjsDefault'), + ), + ), + ), + doc='Delete default class and descendants from mjSpec, remove all references, return 0 on success.', # pylint: disable=line-too-long )), ('mjs_addBody', FunctionDecl( @@ -9954,7 +9974,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionDecl( name='mjs_findDefault', return_type=PointerType( - inner_type=ValueType(name='mjsDefault', is_const=True), + inner_type=ValueType(name='mjsDefault'), ), parameters=( FunctionParameterDecl( diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 41599963..bc9e5299 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -448,7 +448,7 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); mjSpec.def( "find_default", - [](MjSpec& self, std::string& classname) -> const raw::MjsDefault* { + [](MjSpec& self, std::string& classname) -> raw::MjsDefault* { return mjs_findDefault(self.ptr, classname.c_str()); }, py::return_value_policy::reference_internal); @@ -499,6 +499,11 @@ PYBIND11_MODULE(_specs, m) { return mjs_addDefault(spec->ptr, classname.c_str(), parent); }, py::return_value_policy::reference_internal); + mjSpec.def("detach_default", [](MjSpec& self, raw::MjsDefault& def) { + if (mjs_detachDefault(self.ptr, &def) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } + }); mjSpec.def_property_readonly( "default", [](MjSpec& self) -> raw::MjsDefault* { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 4bccae86..c946f420 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -589,6 +589,12 @@ class SpecsTest(absltest.TestCase): spec.compile() self.assertEqual(spec.to_xml(), XML) + # test delete default + def1 = spec.find_default('def1') + spec.detach_default(def1) + def1 = spec.find_default('def1') + self.assertIsNone(def1) + def test_element_list(self): spec = mujoco.MjSpec() sensor1 = spec.add_sensor() diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 937c1ae8..f925410c 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -2137,6 +2137,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) { // -2: invalid objtype static int numObjects(const mjModel* m, mjtObj objtype) { switch (objtype) { + case mjOBJ_DEFAULT: case mjOBJ_FRAME: case mjOBJ_UNKNOWN: return -1; diff --git a/src/user/user_api.cc b/src/user/user_api.cc index a8363919..0e339780 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -227,7 +227,7 @@ const char* mjs_getError(mjSpec* s) { -// Detach body from mjSpec, return 0 if success. +// detach body from mjSpec, return 0 on success int mjs_detachBody(mjSpec* s, mjsBody* b) { mjCModel* model = static_cast(s->element); mjCBody* body = static_cast(b->element); @@ -241,7 +241,22 @@ int mjs_detachBody(mjSpec* s, mjsBody* b) { return 0; } - +// detach default from mjSpec, return 0 on success +int mjs_detachDefault(mjSpec* s, mjsDefault* def) { + mjCModel* modelC = static_cast(s->element); + if (!def) { + modelC->SetError(mjCError(0, "Cannot detach, default is null")); + return -1; + } + mjCDef* defC = static_cast(def->element); + try { + *modelC -= *defC; + } catch (mjCError& e) { + modelC->SetError(e); + return -1; + } + return 0; +} // check if model has warnings int mjs_isWarning(mjSpec* s) { @@ -292,15 +307,19 @@ int mjs_setDeepCopy(mjSpec* s, int deepcopy) { -// delete object, return 0 if success +// delete object, return 0 on success int mjs_delete(mjsElement* element) { - mjCBase* object = static_cast(element); + mjCModel* model; + if (element->elemtype == mjOBJ_DEFAULT) + model = static_cast(element)->model; + else + model = static_cast(element)->model; try { // it will call the appropriate destructor since ~mjCBase is virtual - object->model->DeleteElement(element); + model->DeleteElement(element); return 0; } catch (mjCError& e) { - object->model->SetError(e); + model->SetError(e); return -1; } } @@ -625,7 +644,7 @@ mjsDefault* mjs_getDefault(mjsElement* element) { // Find default with given name in model. -const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) { +mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) { mjCModel* modelC = static_cast(s->element); mjCDef* cdef = modelC->FindDefault(classname); if (!cdef) { diff --git a/src/user/user_api.h b/src/user/user_api.h index 6a05f863..3c6c4420 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -88,6 +88,8 @@ MJAPI mjsFrame* mjs_attachFrameToSite(mjsSite* parent, const mjsFrame* child, // Detach body from mjSpec, remove all references and delete the body, return 0 on success. MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); +// Detach default from mjSpec, remove all references and delete the default, return 0 on success. +MJAPI int mjs_detachDefault(mjSpec* s, mjsDefault* d); //---------------------------------- Add tree elements --------------------------------------------- @@ -221,7 +223,7 @@ MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); MJAPI mjsDefault* mjs_getDefault(mjsElement* element); // Find default in model by class name. -MJAPI const mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); // Get global default from model. MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 76f2a41f..f30c40e5 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -144,7 +145,7 @@ mjCModel::mjCModel() { Clear(); //------------------------ master default set - defaults_.push_back(new mjCDef); + defaults_.push_back(new mjCDef(this)); defaults_.back()->name = "main"; // point to model from spec @@ -564,6 +565,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { mjCModel_& mjCModel::operator+=(mjCDef& subtree) { defaults_.push_back(&subtree); def_map[subtree.name] = &subtree; + subtree.model = this; // set parent to the main default if this is not the only default in the model if (!subtree.parent && &subtree != defaults_[0]) { @@ -579,6 +581,58 @@ mjCModel_& mjCModel::operator+=(mjCDef& subtree) { +// remove default class from array +mjCModel& mjCModel::operator-=(const mjCDef& subtree) { + + // check we aren't trying to remove the 'main' default + if (subtree.id == 0) { + throw mjCError(0, "cannot remove the global default ('main')"); + } + + // remove this default from parent's child list + mjCDef* parent = subtree.parent; + if (parent) { + for (int i = 0; i < parent->child.size(); ++i) { + if (parent->child[i] == &subtree) { + parent->child.erase(parent->child.begin() + i); + break; + } + } + } + + // traverse tree to find all descendants starting from subtree.id + std::vector default_ids_to_remove; + std::vector stack; + stack.push_back(subtree.id); + while (!stack.empty()) { + int id = stack.back(); + stack.pop_back(); + default_ids_to_remove.push_back(id); + for (int i=0; ichild.size(); i++) { + stack.push_back(defaults_[id]->child[i]->id); + } + } + + // remove from the tree + std::sort(default_ids_to_remove.begin(), + default_ids_to_remove.end(), + std::greater()); + + for (int id : default_ids_to_remove) { + delete defaults_[id]; + defaults_.erase(defaults_.begin() + id); + } + + // reset default ids + for (int i = 0; i < defaults_.size(); ++i) { + defaults_[i]->id = i; + } + + return *this; +} + + + template void deletefromlist(std::vector* list, mjsElement* element) { if (!list) { @@ -609,6 +663,10 @@ void mjCModel::DeleteElement(mjsElement* el) { throw mjCError(nullptr, "bodies cannot be deleted, use detach instead"); break; + case mjOBJ_DEFAULT: + throw mjCError(nullptr, "defaults cannot be deleted, use detach instead"); + break; + case mjOBJ_GEOM: { mjCGeom* geom = static_cast(el); @@ -1180,7 +1238,7 @@ mjCDef* mjCModel::AddDefault(string name, mjCDef* parent) { } // create new object - mjCDef* def = new mjCDef; + mjCDef* def = new mjCDef(parent->model); defaults_.push_back(def); def->id = thisid; diff --git a/src/user/user_model.h b/src/user/user_model.h index e5ff19f3..19386280 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -183,10 +183,11 @@ class mjCModel : public mjCModel_, private mjSpec { void CopyFromSpec(); // copy spec to private attributes void PointToLocal(); - mjCModel& operator=(const mjCModel& other); // copy other into this, if they are not the same - mjCModel& operator+=(const mjCModel& other); // add other into this, even if they are the same - mjCModel& operator-=(const mjCBody& subtree); // remove subtree and all references from model - mjCModel_& operator+=(mjCDef& subtree); // add default tree to this model + mjCModel& operator=(const mjCModel& other); // copy other into this, if they are not the same + mjCModel& operator+=(const mjCModel& other); // add other into this, even if they are the same + mjCModel& operator-=(const mjCBody& subtree); // remove subtree and all references from model + mjCModel_& operator+=(mjCDef& subtree); // add default tree to this model + mjCModel& operator-=(const mjCDef& subtree); // remove default tree from this model mjSpec spec; @@ -225,6 +226,9 @@ class mjCModel : public mjCModel_, private mjSpec { // delete object from the corresponding list void DeleteElement(mjsElement* el); + // delete default and all descendants + void RemoveDefault(mjCDef* def); + // detach subtree from model void Detach(mjCBody* subtree); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 624819bb..18c573d2 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -536,12 +536,16 @@ int mjCBoundingVolumeHierarchy::MakeBVH( //------------------------- class mjCDef implementation -------------------------------------------- + + // constructor mjCDef::mjCDef() { name.clear(); id = 0; parent = nullptr; + model = 0; child.clear(); + elemtype = mjOBJ_DEFAULT; mjs_defaultJoint(&joint_.spec); mjs_defaultGeom(&geom_.spec); mjs_defaultSite(&site_.spec); @@ -561,6 +565,13 @@ mjCDef::mjCDef() { +// constructor with model +mjCDef::mjCDef(mjCModel* _model) : mjCDef() { + model = _model; +} + + + // copy constructor mjCDef::mjCDef(const mjCDef& other) { *this = other; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index f8484ddf..47703182 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1866,6 +1866,7 @@ class mjCDef : public mjsElement { public: mjCDef(); + mjCDef(mjCModel*); mjCDef(const mjCDef& other); mjCDef& operator=(const mjCDef& other); mjCDef& operator+=(const mjCDef& other); @@ -1898,6 +1899,7 @@ class mjCDef : public mjsElement { std::vector child; // child classes mjsDefault spec; + mjCModel* model; // pointer to model that owns object private: mjCJoint joint_; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 9762aacf..e5af1bf7 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -2544,6 +2544,91 @@ TEST_F(MujocoTest, ApplyNameSpaceToDefaults) { mj_deleteVFS(vfs.get()); } +TEST_F(MujocoTest, DetachDefault) { + static constexpr char xml_c[] = R"( + + + + + + + + + + + + + + + + + + + + )"; + + static constexpr char cube[] = R"( + v -0.500000 -0.500000 0.500000 + v 0.500000 -0.500000 0.500000 + v -0.500000 0.500000 0.500000 + v 0.500000 0.500000 0.500000 + v -0.500000 0.500000 -0.500000 + v 0.500000 0.500000 -0.500000 + v -0.500000 -0.500000 -0.500000 + v 0.500000 -0.500000 -0.500000)"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube)); + + std::array err; + mjSpec* spec = mj_parseXMLString(xml_c, vfs.get(), err.data(), err.size()); + EXPECT_THAT(spec, NotNull()) << err.data(); + + // get default + mjsDefault* child = mjs_findDefault(spec, "child1"); + EXPECT_THAT(child, NotNull()); + + // try using mjs_delete to remove default, should fail + EXPECT_EQ(mjs_delete(child->element), -1); + + // detach default + EXPECT_EQ(mjs_detachDefault(spec, child), 0); + child = mjs_findDefault(spec, "child1"); + EXPECT_THAT(child, IsNull()); + + // try and detach previously detached default, should fail + EXPECT_EQ(mjs_detachDefault(spec, child), -1); + child = mjs_findDefault(spec, "child1"); + EXPECT_THAT(child, IsNull()); + EXPECT_THAT(mjs_getError(spec), + HasSubstr("Cannot detach, default is null")); + + // detach parent + mjsDefault* parent = mjs_findDefault(spec, "parent"); + EXPECT_THAT(parent, NotNull()); + mjs_detachDefault(spec, parent); + + // both parent and remaining child should be removed + parent = mjs_findDefault(spec, "parent"); + EXPECT_THAT(parent, IsNull()); + child = mjs_findDefault(spec, "child2"); + EXPECT_THAT(child, IsNull()); + + // error when trying to detach the 'main' default + mjsDefault* main = mjs_findDefault(spec, "main"); + EXPECT_THAT(main, NotNull()); + EXPECT_EQ(mjs_detachDefault(spec, main), -1); + EXPECT_THAT(mjs_getError(spec), + HasSubstr("cannot remove the global default ('main')")); + + main = mjs_findDefault(spec, "main"); + EXPECT_THAT(main, NotNull()); + + mj_deleteVFS(vfs.get()); + mj_deleteSpec(spec); +} + TEST_F(MujocoTest, ErrorWhenCompilingOrphanedSpec) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c529224c..845c5c91 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -319,6 +319,7 @@ public enum mjtObj : int{ mjOBJ_PLUGIN = 25, mjNOBJECT = 26, mjOBJ_FRAME = 100, + mjOBJ_DEFAULT = 101, } public enum mjtConstraint : int{ mjCNSTR_EQUALITY = 0,