From 27b9ddda5bdd12345abdfbc0a6efce121d40cd6c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 10 Jul 2024 12:40:50 -0700 Subject: [PATCH] Add attach tag to MJCF. PiperOrigin-RevId: 651117953 Change-Id: I57b0e53ed44607d479b3785d908e0837c0804cf3 --- doc/XMLreference.rst | 105 +++++++++++---- doc/XMLschema.rst | 14 ++ doc/changelog.rst | 27 ++-- model/humanoid/22_humanoids.xml | 199 +---------------------------- src/user/user_api.cc | 18 +++ src/user/user_api.h | 6 + src/user/user_model.cc | 26 +++- src/user/user_model.h | 5 +- src/user/user_objects.cc | 15 +++ src/xml/xml.cc | 2 +- src/xml/xml_base.h | 3 +- src/xml/xml_native_reader.cc | 64 +++++++++- src/xml/xml_native_reader.h | 6 +- src/xml/xml_urdf.cc | 2 +- src/xml/xml_urdf.h | 3 +- test/user/user_api_test.cc | 12 +- test/xml/testdata/child.xml | 7 + test/xml/testdata/parent.xml | 11 ++ test/xml/xml_native_reader_test.cc | 142 +++++++++++++++++++- 19 files changed, 419 insertions(+), 248 deletions(-) create mode 100644 test/xml/testdata/child.xml create mode 100644 test/xml/testdata/parent.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index b0dfd334bd..16368e69f0 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -110,11 +110,11 @@ Meta elements These elements are not strictly part of the low-level MJCF format definition, but rather instruct the compiler to perform some operation on the model. A general property of meta-elements is that they disappear from the model upon -saving the XML. There are currently five meta-elements in MJCF: +saving the XML. There are currently six meta-elements in MJCF: - :ref:`include`, :ref:`frame`, and :ref:`replicate` which are outside of the schema. -- :ref:`composite` and :ref:`flexcomp` which are part of the schema, but serve to - procedurally generate other MJCF elements. +- :ref:`composite`, :ref:`flexcomp` and :ref:`attach` which are part of the + schema, but serve to procedurally generate other MJCF elements. .. _frame: @@ -1739,6 +1739,24 @@ properties are grouped together. definition could in fact come from a defaults class. The remaining material properties always apply. +.. _asset-model: + +:el-prefix:`asset/` |-| **model** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This element specifies other MJCF models which may be used for :ref:`attachment` in the current model. + +.. _asset-model-name: + +:at:`name`: :at-val:`string, required` + Name of the sub-model, used for referencing in :ref:`attach`. If unspecified, the + :ref:`model name` is used. + +.. _asset-model-file: + +:at:`file`: :at-val:`string, required` + The file from which the sub-model will be loaded. Note that the sub-model must be a valid MJCF model. + + .. _body: **(world)body** (R) @@ -2816,24 +2834,6 @@ the direction specified by the dir attribute. It does not have a full spatial fr The specular color of the light. -.. _body-plugin: - -:el-prefix:`body/` |-| **plugin** (?) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Associate this body with an :ref:`engine plugin`. Either :at:`plugin` or :at:`instance` are required. - -.. _body-plugin-plugin: - -:at:`plugin`: :at-val:`string, optional` - Plugin identifier, used for implicit plugin instantiation. - -.. _body-plugin-instance: - -:at:`instance`: :at-val:`string, optional` - Instance name, used for explicit plugin instantiation. - - .. _body-composite: :el-prefix:`body/` |-| **composite** (*) @@ -3638,10 +3638,71 @@ Associate this flexcomp with an :ref:`engine plugin`. Either :at:`plug Instance name, used for explicit plugin instantiation. +.. _body-plugin: + +:el-prefix:`body/` |-| **plugin** (?) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Associate this body with an :ref:`engine plugin`. Either :at:`plugin` or :at:`instance` are required. + +.. _body-plugin-plugin: + +:at:`plugin`: :at-val:`string, optional` + Plugin identifier, used for implicit plugin instantiation. + +.. _body-plugin-instance: + +:at:`instance`: :at-val:`string, optional` + Instance name, used for explicit plugin instantiation. + + +.. _body-attach: + +:el-prefix:`body/` |-| **attach** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The :el:`attach` element is used to insert a sub-tree of bodies from another model into this model's kinematic tree. +Unlike :ref:`include`, which is implemented in the parser and is equivalent to copying and pasting XML from +one file into another, :el:`attach` is implemented in the model compiler. In order to use this element, the sub-model +must first be defined as an :ref:`asset`. When creating an attachment, the top body of the attached subtree +is specified, and all referencing elements outside the kinematic tree (e.g., sensors and actuators), are +also copied into the top-level model. Additionally, any elements referenced from within the attached subtree (e.g. +defaults and assets) will be copied in to the top-level model. :el:`attach` is a :ref:`meta-element`, so upon saving +all attachments will appear in the saved XML file. + +.. admonition:: Known issues + :class: attention + + The :el:`attach` meta-element is new and not well tested. Please report any issues you encounter to the development + team. Additionally, the following known limitations exist, to be addressed in a future release: + + - The world body cannot be attached. + - An entire model cannot be attached (i.e. including all elements, referenced or not). + - All assets from the child model will be copied in, whether they are referenced or not. + - Self-attach or circular references are not checked for and will lead to infinite loops. + - :ref:`Keyframes` are not yet supported. When attaching, all keyframes will be deleted. + +.. _body-attach-model: + +:at:`model`: :at-val:`string, optional` + The sub-model from which to attach a subtree. + +.. _body-attach-body: + +:at:`body`: :at-val:`string, optional` + Name of the body in the sub-model to attach here. The body and its subtree will be attached. + +.. _body-attach-prefix: + +:at:`prefix`: :at-val:`string, optional` + Prefix to prepend to names of elements in the sub-model. If empty, the names are unchanged. This attribute is + required to prevent name collisions with the parent or when attaching the same sub-tree multiple times. + + .. _body-frame: :el-prefix:`body/` |-| **frame** (*) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Frames specify a coordinate transformation which is applied to all child elements. They disappear during compilation and the transformation they encode is accumulated in their direct children. See :ref:`frame` for examples. diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 37b1dd5dd7..e6dc1b77c0 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -177,6 +177,13 @@ | | | | :ref:`reflectance` | :ref:`metallic` | :ref:`roughness` | :ref:`rgba` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| asset |br| |_| |L| | | .. table:: | +| :ref:`model | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`file` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | .. table:: | | :ref:`body | R | :class: mjcf-attributes | | ` | | | @@ -261,6 +268,13 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | +| :ref:`attach | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`model` | :ref:`body` | :ref:`prefix` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| body |br| |_| |L| | | .. table:: | | :ref:`site | \* | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | diff --git a/doc/changelog.rst b/doc/changelog.rst index 54be087b77..8cc342916d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -39,27 +39,30 @@ General :align: right :width: 240px -7. Added support for orthographic cameras. This is available for both fixed cameras and the free camera, using the +7. Add :ref:`attach` meta-element to MJCF, which allows attaching a model to a body. +8. Added support for orthographic cameras. This is available for both fixed cameras and the free camera, using the :ref:`camera/orthographic` and :ref:`global/orthographic` attributes, respectively. -8. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. -9. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. -10. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). -11. Replaced ``mjUSEDOUBLE`` which was previously hard-coded in +9. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. +10. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. +11. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). +12. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and + :ref:`mju_mulMatTVec3`. These functions names and argument ordering are more consistent with the rest of the API. +13. Replaced ``mjUSEDOUBLE`` which was previously hard-coded in `mjtnum.h `__ with the build-time flag ``mjUSESINGLE``. If this symbol is not defined, MuJoCo will use double-precision floating point, as usual. If ``mjUSESINGLE`` is defined, MuJoCo will use single-precision floating point. See :ref:`mjtNum`. Relatedly, fixed various type errors that prevented building with single-precision. -12. Quaternions in ``mjData->qpos`` and ``mjData->mocap_quat`` are no longer normalized in-place by +14. Quaternions in ``mjData->qpos`` and ``mjData->mocap_quat`` are no longer normalized in-place by :ref:`mj_kinematics`. Instead they are normalized when they are used. After the first step, quaternions in ``mjData->qpos`` will be normalized. MJX ~~~ -13. Added support for :ref:`elliptic friction cones`. -14. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. -15. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. +15. Added support for :ref:`elliptic friction cones`. +16. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. +17. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. .. youtube:: P83tKA1iz2Y @@ -68,14 +71,14 @@ MJX Simulate ^^^^^^^^ -16. Added improved tutorial video. -17. Improved the Brownian noise generator. +18. Added improved tutorial video. +19. Improved the Brownian noise generator. |br| |br| |br| |br| Python bindings ^^^^^^^^^^^^^^^ -18. Fixed a memory leak when using ``copy.deepcopy()`` on a ``mujoco.MjData`` instance (:github:issue:`1572`). +20. Fixed a memory leak when using ``copy.deepcopy()`` on a ``mujoco.MjData`` instance (:github:issue:`1572`). Version 3.1.6 (Jun 3, 2024) --------------------------- diff --git a/model/humanoid/22_humanoids.xml b/model/humanoid/22_humanoids.xml index 5050941f53..700f392d0d 100644 --- a/model/humanoid/22_humanoids.xml +++ b/model/humanoid/22_humanoids.xml @@ -20,82 +20,11 @@ - - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -104,127 +33,11 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 4a3af4401e..62e0bd5e23 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -101,6 +101,7 @@ int mjs_attachBody(mjsFrame* parent, const mjsBody* child, try { *frame_parent += std::string(prefix) + *child_body + std::string(suffix); } catch (mjCError& e) { + frame_parent->model->SetError(e); return -1; } return 0; @@ -116,6 +117,7 @@ int mjs_attachFrame(mjsBody* parent, const mjsFrame* child, try { *body_parent += std::string(prefix) + *child_frame + std::string(suffix); } catch (mjCError& e) { + body_parent->model->SetError(e); return -1; } return 0; @@ -158,6 +160,14 @@ void mj_deleteSpec(mjSpec* s) { +// add spec (model asset) to spec +void mjs_addSpec(mjSpec* s, mjSpec* child) { + mjCModel* model = static_cast(s->element); + model->AppendSpec(child); +} + + + // delete object, it will call the appropriate destructor since ~mjCBase is virtual void mjs_delete(mjsElement* element) { mjCBase* object = static_cast(element); @@ -466,6 +476,14 @@ mjSpec* mjs_getSpec(mjsBody* body) { +// find spec (model asset) by name +mjSpec* mjs_findSpec(mjSpec* s, const char* name) { + mjCModel* model = static_cast(s->element); + return model->FindSpec(name); +} + + + // get default mjsDefault* mjs_getDefault(mjsElement* element) { mjCModel* model = static_cast(element)->model; diff --git a/src/user/user_api.h b/src/user/user_api.h index 7c870ae2b0..d502261017 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -60,6 +60,9 @@ MJAPI void mj_copyBack(mjSpec* s, const mjModel* m); // Delete spec. MJAPI void mj_deleteSpec(mjSpec* s); +// Add spec (model asset) to spec. +MJAPI void mjs_addSpec(mjSpec* s, mjSpec* child); + //---------------------------------- Attachment ---------------------------------------------------- @@ -182,6 +185,9 @@ MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def); // Get spec from body. MJAPI mjSpec* mjs_getSpec(mjsBody* body); +// Find spec (model asset) by name. +MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name); + // Find body in model by name. MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8505be08e2..7a81c0f4be 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include #include "cc/array_safety.h" #include "engine/engine_forward.h" #include "engine/engine_io.h" @@ -338,6 +338,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { // add default tree to this model mjCModel_& mjCModel::operator+=(mjCDef& subtree) { defaults_.push_back(&subtree); + def_map[subtree.name] = &subtree; // set parent to the main default if this is not the only default in the model if (!subtree.parent && &subtree != defaults_[0]) { @@ -660,6 +661,11 @@ mjCPlugin* mjCModel::AddPlugin() { } +// append spec to spec +void mjCModel::AppendSpec(mjSpec* spec) { + specs_.push_back(spec); +} + //------------------------ API FOR ACCESS TO MODEL ELEMENTS --------------------------------------- @@ -886,6 +892,18 @@ mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) const{ +// find spec by name +mjSpec* mjCModel::FindSpec(std::string name) const { + for (auto spec : specs_) { + if (mjs_getString(spec->modelname) == name) { + return spec; + } + } + return nullptr; +} + + + // detect null pose bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) const { bool result = true; @@ -3247,6 +3265,12 @@ mjModel* mjCModel::Compile(const mjVFS* vfs, mjModel** m) { return nullptr; } + // destroy attached specs + for (auto spec : specs_) { + mj_deleteSpec(spec); + } + specs_.clear(); + // restore error handler, mark as compiled, return mjModel _mjPRIVATE__set_tls_error_fn(save_error); _mjPRIVATE__set_tls_warning_fn(save_warning); diff --git a/src/user/user_model.h b/src/user/user_model.h index 296bd6dfb1..d04b7a7e80 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -192,7 +192,7 @@ class mjCModel : public mjCModel_, private mjSpec { mjCTuple* AddTuple(); mjCKey* AddKey(); mjCPlugin* AddPlugin(); - + void AppendSpec(mjSpec* spec); // delete elements marked as discard=true template void Delete(std::vector& elements, @@ -209,12 +209,14 @@ class mjCModel : public mjCModel_, private mjSpec { // API for access to other variables bool IsCompiled() const; // is model already compiled const mjCError& GetError() const; // get reference of error object + void SetError(const mjCError& error) { errInfo = error; } // set value of error object mjCBody* GetWorld(); // pointer to world body mjCDef* FindDefault(std::string name); // find defaults class name mjCDef* AddDefault(std::string name, mjCDef* parent = nullptr); // add defaults class to array mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name mjCBody* FindBody(mjCBody* body, std::string name); // find body given name mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name + mjSpec* FindSpec(std::string name) const; // find spec given name bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose void SetActivePlugins(const std::vector>&& active_plugins) { active_plugins_ = std::move(active_plugins); @@ -312,6 +314,7 @@ class mjCModel : public mjCModel_, private mjSpec { std::vector tuples_; // list of tuple fields std::vector keys_; // list of keyframe fields std::vector plugins_; // list of plugin instances + std::vector specs_; // list of specs // pointers to objects created inside kinematic tree std::vector bodies_; // list of bodies diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 8c17ead315..2f5069406c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -697,6 +697,9 @@ void mjCBase::NameSpace(const mjCModel* m) { if (!name.empty()) { name = m->prefix + name + m->suffix; } + if (!classname.empty() && m != model) { + classname = m->prefix + classname + m->suffix; + } } @@ -1000,6 +1003,9 @@ void mjCBody::NameSpace_(const mjCModel* m, bool propagate) { if (!name.empty()) { name = m->prefix + name + m->suffix; } + if (!classname.empty() && m != model) { + classname = m->prefix + classname + m->suffix; + } for (auto& body : bodies) { body->prefix = m->prefix; @@ -2004,6 +2010,9 @@ void mjCGeom::NameSpace(const mjCModel* m) { if (!name.empty()) { name = m->prefix + name + m->suffix; } + if (!classname.empty() && m != model) { + classname = m->prefix + classname + m->suffix; + } if (!spec_material_.empty() && model != m) { spec_material_ = m->prefix + spec_material_ + m->suffix; } @@ -2768,6 +2777,9 @@ void mjCCamera::NameSpace(const mjCModel* m) { if (!name.empty()) { name = m->prefix + name + m->suffix; } + if (!classname.empty() && m != model) { + classname = m->prefix + classname + m->suffix; + } if (!spec_targetbody_.empty()) { spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix; } @@ -2917,6 +2929,9 @@ void mjCLight::NameSpace(const mjCModel* m) { if (!name.empty()) { name = m->prefix + name + m->suffix; } + if (!classname.empty() && m != model) { + classname = m->prefix + classname + m->suffix; + } if (!spec_targetbody_.empty()) { spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix; } diff --git a/src/xml/xml.cc b/src/xml/xml.cc index 5c0349d538..7ec8243af8 100644 --- a/src/xml/xml.cc +++ b/src/xml/xml.cc @@ -369,7 +369,7 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs, // parse MuJoCo model parser.SetModel(spec); - parser.Parse(root); + parser.Parse(root, vfs); } else if (!strcasecmp(root->Value(), "robot")) { diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index ec3fd74c98..c9afec771d 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -19,6 +19,7 @@ #include #include "tinyxml2.h" +#include #include #include "xml/xml_util.h" @@ -79,7 +80,7 @@ class mjXBase : public mjXUtil { virtual ~mjXBase() = default; // parse: implemented in derived parser classes - virtual void Parse(tinyxml2::XMLElement* root) {}; + virtual void Parse(tinyxml2::XMLElement* root, const mjVFS* vfs = nullptr) {}; // write: implemented in derived writer class virtual std::string Write(char *error, std::size_t error_sz) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 953988caeb..fe89c15167 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -14,6 +14,7 @@ #include "xml/xml_native_reader.h" +#include #include #include #include @@ -38,6 +39,7 @@ #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include +#include "user/user_api.h" #include "user/user_composite.h" #include "user/user_flexcomp.h" #include "user/user_util.h" @@ -243,6 +245,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "hflip", "vflip"}, {"material", "*", "12", "name", "class", "texture", "texrepeat", "texuniform", "emission", "specular", "shininess", "reflectance", "metallic", "roughness", "rgba"}, + {"model", "*", "2", "name", "file"}, {">"}, {"body", "R", "11", "name", "childclass", "pos", "quat", "mocap", @@ -267,6 +270,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"config", "*", "2", "key", "value"}, {">"}, {">"}, + {"attach", "*", "3", "model", "body", "prefix"}, {"site", "*", "15", "name", "class", "type", "group", "pos", "quat", "material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"}, {"camera", "*", "20", "name", "class", "orthographic", "fovy", "ipd", "resolution", "pos", @@ -805,7 +809,7 @@ void mjXReader::PrintSchema(std::stringstream& str, bool html, bool pad) { // main entry point for XML parser // mjCModel is allocated here; caller is responsible for deallocation -void mjXReader::Parse(XMLElement* root) { +void mjXReader::Parse(XMLElement* root, const mjVFS* vfs) { // check schema if (!schema.GetError().empty()) { throw mjXError(0, "XML Schema Construction Error: %s\n", @@ -880,7 +884,7 @@ void mjXReader::Parse(XMLElement* root) { for (XMLElement* section = FirstChildElement(root, "asset"); section; section = NextSiblingElement(section, "asset")) { - Asset(section); + Asset(section, vfs); } for (XMLElement* section = FirstChildElement(root, "contact"); section; @@ -3045,7 +3049,7 @@ void mjXReader::Visual(XMLElement* section) { // asset section parser -void mjXReader::Asset(XMLElement* section) { +void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { int n; string text, name, texname, content_type; XMLElement* elem; @@ -3209,6 +3213,27 @@ void mjXReader::Asset(XMLElement* section) { } } + // model sub-element + else if (name=="model") { + auto filename = modelfiledir_ + ReadAttrFile(elem, "file", "").value(); + + // parse the child + std::array error; + mjSpec* child = mj_parseXML(filename.c_str(), vfs, error.data(), error.size()); + if (!child) { + throw mjXError(elem, "could not parse model file with error: %s", error.data()); + } + + // overwrite model name if given + std::string modelname = ""; + if (ReadAttrTxt(elem, "name", modelname)) { + mjs_setString(child->modelname, modelname.c_str()); + } + + // store child spec in model + mjs_addSpec(model, child); + } + // advance to next element elem = NextSiblingElement(elem); } @@ -3437,8 +3462,8 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) { Body(elem, subtree, pframe); // attach to parent - if (mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str()) < 0) { - throw mjXError(elem, "failed to attach frame"); + if (mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str()) != 0) { + throw mjXError(elem, mjs_getError(model)); } } @@ -3496,6 +3521,35 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) { Body(elem, pchild, nullptr); } + // attachment + else if (name=="attach") { + std::string model_name, body_name, prefix; + ReadAttrTxt(elem, "model", model_name); + ReadAttrTxt(elem, "body", body_name); + ReadAttrTxt(elem, "prefix", prefix); + + mjsBody* child = mjs_findBody(model, (prefix+body_name).c_str()); + mjsFrame* pframe = frame ? frame : mjs_addFrame(pbody, nullptr); + + if (!child) { + mjSpec* asset = mjs_findSpec(model, model_name.c_str()); + if (!asset) { + throw mjXError(0, "could not find model '%s'", model_name.c_str()); + } + child = mjs_findBody(asset, body_name.c_str()); + if (!child) { + throw mjXError(0, "could not find body '%s''%s'", body_name.c_str()); + } + if (mjs_attachBody(pframe, child, prefix.c_str(), "") != 0) { + mj_deleteSpec(asset); + throw mjXError(elem, mjs_getError(model)); + } + } else { + // only set frame to existing body + mjs_setFrame(child->element, pframe); + } + } + // no match else { throw mjXError(elem, "unrecognized model element '%s'", name.c_str()); diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index f2049becef..11743df7df 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -30,7 +30,7 @@ class mjXReader : public mjXBase { mjXReader(); // constructor virtual ~mjXReader() = default; // destructor - void Parse(tinyxml2::XMLElement* root); // parse XML document + void Parse(tinyxml2::XMLElement* root, const mjVFS* vfs = nullptr); // parse XML document void PrintSchema(std::stringstream& str, bool html, bool pad); // print text or HTML schema void SetModelFileDir(std::string modelfiledir); @@ -53,7 +53,7 @@ class mjXReader : public mjXBase { void Custom(tinyxml2::XMLElement* section); // custom section void Visual(tinyxml2::XMLElement* section); // visual section void Statistic(tinyxml2::XMLElement* section); // statistic section - void Asset(tinyxml2::XMLElement* section); // asset section + void Asset(tinyxml2::XMLElement* section, const mjVFS* vfs); // asset section void Body(tinyxml2::XMLElement* section, mjsBody* pbody, mjsFrame* pframe); // body/world section void Contact(tinyxml2::XMLElement* section); // contact section @@ -99,7 +99,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 230 +#define nMJCF 232 extern const char* MJCF[nMJCF][mjXATTRNUM]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 628388e178..4b2317c1c4 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -364,7 +364,7 @@ void mjXURDF::Body(XMLElement* body_elem) { } } -void mjXURDF::Parse(XMLElement* root) { +void mjXURDF::Parse(XMLElement* root, const mjVFS* vfs) { double pos[3] = {0}; mjuu_setvec(pos, 0, 0, 0); double quat[4] = {1, 0, 0, 0}; diff --git a/src/xml/xml_urdf.h b/src/xml/xml_urdf.h index b549ac6e61..2a3f682f09 100644 --- a/src/xml/xml_urdf.h +++ b/src/xml/xml_urdf.h @@ -19,6 +19,7 @@ #include #include +#include #include #include "xml/xml_base.h" #include "tinyxml2.h" @@ -41,7 +42,7 @@ class mjXURDF : public mjXBase { double* pos, double* quat, bool static_body); - void Parse(tinyxml2::XMLElement* root); // main parser + void Parse(tinyxml2::XMLElement* root, const mjVFS* vfs = nullptr); // main parser private: std::string GetPrefixedName(const std::string& name); // get prefix/name of element diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 66da62fccb..64d87650d7 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -502,8 +502,8 @@ TEST_F(MujocoTest, AttachDifferent) { - - + + @@ -618,8 +618,8 @@ TEST_F(MujocoTest, AttachFrame) { - - + + @@ -650,7 +650,7 @@ TEST_F(MujocoTest, AttachFrame) { mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size()); EXPECT_THAT(parent, NotNull()) << er.data(); - // get frame + // get body mjsBody* body = mjs_findBody(parent, "sphere"); EXPECT_THAT(body, NotNull()); @@ -662,7 +662,7 @@ TEST_F(MujocoTest, AttachFrame) { mjsFrame* frame = mjs_findFrame(child, "pframe"); EXPECT_THAT(frame, NotNull()); - // attach child to parent frame + // attach child frame to parent body EXPECT_THAT( mjs_attachFrame(body, frame, /*prefix=*/"attached-", /*suffix=*/"-1"), 0); diff --git a/test/xml/testdata/child.xml b/test/xml/testdata/child.xml new file mode 100644 index 0000000000..2d63790b01 --- /dev/null +++ b/test/xml/testdata/child.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/xml/testdata/parent.xml b/test/xml/testdata/parent.xml new file mode 100644 index 0000000000..f0c112b825 --- /dev/null +++ b/test/xml/testdata/parent.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index f6ae02f78e..1da989e322 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1054,6 +1054,7 @@ TEST_F(XMLReaderTest, DuplicateFrameName) { // ---------------------- test replicate parsing ------------------------------- + TEST_F(XMLReaderTest, ParseReplicate) { static constexpr char xml[] = R"( @@ -1253,7 +1254,146 @@ TEST_F(XMLReaderTest, ParseReplicateRepeatedName) { std::array error; mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size()); EXPECT_THAT(spec, IsNull()) << error.data(); - EXPECT_THAT(error.data(), HasSubstr("failed to attach frame")); + EXPECT_THAT(error.data(), HasSubstr("repeated name 'b' in actuator")); + EXPECT_THAT(error.data(), HasSubstr("Element 'replicate'")); +} + +// ---------------------- test spec assets parsing ----------------------------- + +TEST_F(XMLReaderTest, ParseSpecAssets) { + std::array er; + + static const char* const kParentPath = + "xml/testdata/parent.xml"; + static const char* const kChildPath = + "xml/testdata/child.xml"; + + const std::string xml_parent = GetTestDataFilePath(kParentPath); + const std::string xml_child = GetTestDataFilePath(kChildPath); + + mjModel* parent = mj_loadXML(xml_parent.c_str(), 0, er.data(), er.size()); + EXPECT_THAT(parent, NotNull()) << er.data(); + + mjModel* child = mj_loadXML(xml_child.c_str(), 0, er.data(), er.size()); + EXPECT_THAT(child, NotNull()) << er.data(); + + mj_deleteModel(parent); + mj_deleteModel(child); +} + +TEST_F(XMLReaderTest, AttachSpecAssets) { + static constexpr char xml_parent[] = R"( + + + + + + + + + + + + + + )"; + + static constexpr char xml_child[] = R"( + + + + + + + + )"; + + static constexpr char xml_expected[] = R"( + + + + + + + + + + + + + + )"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "xml_child.xml", xml_child, sizeof(xml_child)); + + std::array er; + mjModel* model = + LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); + EXPECT_THAT(model, NotNull()) << er.data(); + + mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(expected, NotNull()) << er.data(); + + mjtNum tol = 0; + std::string field = ""; + EXPECT_LE(CompareModel(model, expected, field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n';; + + mj_deleteModel(model); + mj_deleteModel(expected); + mj_deleteVFS(vfs.get()); +} + +TEST_F(XMLReaderTest, InvalidAttach) { + static constexpr char xml_parent[] = R"( + + + + + + + + + + + + + + + + + )"; + + static constexpr char xml_child[] = R"( + + + + + + + + + + + + + )"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "child.xml", xml_child, sizeof(xml_child)); + + std::array er; + mjModel* model = + LoadModelFromString(xml_parent, er.data(), er.size(), vfs.get()); + + EXPECT_THAT(model, IsNull()) << er.data(); + EXPECT_THAT(er.data(), HasSubstr("repeated name 'actuator' in actuator")); + EXPECT_THAT(er.data(), HasSubstr("Element 'attach'")); + mj_deleteVFS(vfs.get()); } // ----------------------- test camera parsing ---------------------------------