From bda03af49ae5bedc77eaf18b6f0caab1347a91f1 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 13 Mar 2024 11:29:50 -0700 Subject: [PATCH] Add childclass support to frames. PiperOrigin-RevId: 615487831 Change-Id: I0e4d2efd52211701b4286a0b0de84424470a3a47 --- doc/XMLreference.rst | 16 +++++++-- doc/includes/references.h | 5 ++- include/mujoco/mjmodel.h | 5 ++- introspect/enums.py | 1 + src/engine/engine_io.c | 1 + src/engine/engine_util_misc.c | 3 ++ src/user/user_api.h | 4 ++- src/user/user_model.cc | 28 +++++++++------ src/user/user_model.h | 1 + src/user/user_objects.cc | 4 ++- src/user/user_objects.h | 2 ++ src/xml/xml_native_reader.cc | 24 +++++++++++-- src/xml/xml_native_writer.cc | 28 +++++++++++++-- src/xml/xml_native_writer.h | 34 +++++++++---------- test/xml/xml_native_reader_test.cc | 51 ++++++++++++++++++++++++---- test/xml/xml_native_writer_test.cc | 33 ++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 1 + 17 files changed, 195 insertions(+), 46 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 9caf2fd5..dba9aa79 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2174,8 +2174,8 @@ defined. Its body name is automatically defined as "world". :at:`childclass`: :at-val:`string, optional` If this attribute is present, all descendant elements that admit a defaults class will use the class specified here, - unless they specify their own class or another body with a childclass attribute is encountered along the chain of - nested bodies. Recall :ref:`CDefault`. + unless they specify their own class or another body or frame with a childclass attribute is encountered along the + chain of nested bodies and frames. Recall :ref:`CDefault`. .. _body-mocap: @@ -4029,6 +4029,18 @@ Associate this flexcomp with an :ref:`engine plugin`. Either :at:`plug 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. +.. _frame-name: + +:at:`name`: :at-val:`string, optional` + Name of the frame. + +.. _frame-childclass: + +:at:`childclass`: :at-val:`string, optional` + If this attribute is present, all descendant elements that admit a defaults class will use the class specified here, + unless they specify their own class or another frame or body with a childclass attribute is encountered along the + chain of nested bodies and frames. Recall :ref:`CDefault`. + .. _frame-pos: :at:`pos`: :at-val:`real(3), "0 0 0"` diff --git a/doc/includes/references.h b/doc/includes/references.h index 4c48acbd..6fd2a650 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -572,7 +572,10 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_KEY, // keyframe mjOBJ_PLUGIN, // plugin instance - mjNOBJECT // number of object types + mjNOBJECT, // number of object types + + // meta elements, do not appear in mjModel + mjOBJ_FRAME = 100 // frame } mjtObj; typedef enum mjtConstraint_ { // type of constraint mjCNSTR_EQUALITY = 0, // equality constraint diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9055dca0..15840a82 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -250,7 +250,10 @@ typedef enum mjtObj_ { // type of MujoCo object mjOBJ_KEY, // keyframe mjOBJ_PLUGIN, // plugin instance - mjNOBJECT // number of object types + mjNOBJECT, // number of object types + + // meta elements, do not appear in mjModel + mjOBJ_FRAME = 100 // frame } mjtObj; diff --git a/introspect/enums.py b/introspect/enums.py index 180c157b..a1fab80a 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -267,6 +267,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjOBJ_KEY', 24), ('mjOBJ_PLUGIN', 25), ('mjNOBJECT', 26), + ('mjOBJ_FRAME', 100), ]), )), ('mjtConstraint', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index dbba8957..315884ac 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1811,6 +1811,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_FRAME: case mjOBJ_UNKNOWN: return -1; case mjOBJ_BODY: diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 876fe52a..7199555e 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1060,6 +1060,9 @@ const char* mju_type2Str(int type) { case mjOBJ_PLUGIN: return "plugin"; + case mjOBJ_FRAME: + return "frame"; + default: return 0; } diff --git a/src/user/user_api.h b/src/user/user_api.h index af5eb857..47b2541b 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -156,7 +156,7 @@ typedef struct _mjmPlugin { // plugin specification typedef struct _mjmBody { // body specification mjElement element; // internal, do not modify mjString name; // name - mjString classname; // childclass name + mjString childclass; // childclass name // body frame double pos[3]; // frame position @@ -183,6 +183,8 @@ typedef struct _mjmBody { // body specification typedef struct _mjmFrame { // frame specification mjElement element; // internal, do not modify + mjString name; // name + mjString childclass; // childclass name double pos[3]; // position double quat[4]; // orientation mjmOrientation alt; // alternative orientation diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7f424da9..c5426ac0 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -636,6 +636,7 @@ void mjCModel::MakeLists(mjCBody* body) { for (int i=0; isites.size(); i++) sites.push_back(body->sites[i]); for (int i=0; icameras.size(); i++) cameras.push_back(body->cameras[i]); for (int i=0; ilights.size(); i++) lights.push_back(body->lights[i]); + for (int i=0; iframes.size(); i++) frames.push_back(body->frames[i]); // recursive call to all child bodies for (int i=0; ibodies.size(); i++) MakeLists(body->bodies[i]); @@ -2679,18 +2680,20 @@ static void reassignid(vector& list) { template static void processlist(mjListKeyMap& ids, vector& list, mjtObj type, bool checkrepeat = true) { - // loop over list elements - for (size_t i=0; i < list.size(); i++) { - // check for incompatible id setting; SHOULD NOT OCCUR - if (list[i]->id!=-1 && list[i]->id!=i) { - throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i); + // assign ids for regular elements + if (type < mjNOBJECT) { + for (size_t i=0; i < list.size(); i++) { + // check for incompatible id setting; SHOULD NOT OCCUR + if (list[i]->id!=-1 && list[i]->id!=i) { + throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i); + } + + // id equals position in array + list[i]->id = i; + + // add to ids map + ids[type][list[i]->name] = i; } - - // id equals position in array - list[i]->id = i; - - // add to ids map - ids[type][list[i]->name] = i; } // check for repeated names @@ -2855,6 +2858,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { } } + // check repeated names in meta elements + processlist(ids, frames, mjOBJ_FRAME); + // delete visual assets if (discardvisual) { DeleteAll(materials); diff --git a/src/user/user_model.h b/src/user/user_model.h index 6aceefc4..629ec578 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -250,6 +250,7 @@ class mjCModel : private mjSpec { std::vector sites; // list of sites attached to this body std::vector cameras; // list of cameras std::vector lights; // list of lights + std::vector frames; // list of frames // array of pointers to each object list (enumerated by type) std::array*, mjNOBJECT> object_lists; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index c3d81935..db38cad3 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -765,7 +765,7 @@ mjCBody& mjCBody::operator=(const mjCBody& other) { void mjCBody::PointToLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; - spec.classname = (mjString)&classname; + spec.childclass = (mjString)&classname; spec.userdata = (mjDoubleVec)&spec_userdata_; spec.plugin.name = (mjString)&plugin_name; spec.plugin.instance_name = (mjString)&plugin_instance_name; @@ -1375,6 +1375,8 @@ mjCFrame& mjCFrame::operator=(const mjCFrame& other) { void mjCFrame::PointToLocal() { spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.childclass = (mjString)&classname; spec.info = (mjString)&info; } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 79ab83cb..988a084d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -321,6 +321,8 @@ class mjCFrame : public mjCFrame_, private mjmFrame { public: mjmFrame spec; + using mjCBase::name; + using mjCBase::classname; using mjCBase::info; void CopyFromSpec(void); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 1bcf5b7d..4d72f0b0 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -3228,7 +3228,7 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) { // get class if specified, otherwise use body mjmDefault* def = GetClass(elem); if (!def) { - def = (mjmDefault*)mjm_getDefault(pbody->element); + def = mjm_getDefault(frame ? frame->element : pbody->element); } // inertial sub-element @@ -3335,9 +3335,29 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) { // frame sub-element else if (name=="frame") { + // read childdef + mjmDefault* childdef = 0; + if (ReadAttrTxt(elem, "childclass", text)) { + childdef = mjm_findDefault(model, text.c_str()); + mjm_findDefault(model, text.c_str()); + if (!childdef) { + throw mjXError(elem, "unknown default childclass"); + } + } + + // create frame mjmFrame* pframe = mjm_addFrame(pbody, frame); mjm_setString(pframe->info, ("line = " + std::to_string(elem->GetLineNum())).c_str()); + mjm_setDefault(pframe->element, childdef ? childdef : def); + // read attributes + std::string name, childclass; + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pframe->name, name.c_str()); + } + if (ReadAttrTxt(elem, "childclass", childclass)) { + mjm_setString(pframe->childclass, childclass.c_str()); + } ReadAttr(elem, "pos", 3, pframe->pos, text); ReadQuat(elem, "quat", pframe->quat, text); ReadAlternative(elem, pframe->alt); @@ -3368,7 +3388,7 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) { mjm_setString(pchild->name, name.c_str()); } if (ReadAttrTxt(elem, "childclass", childclass)) { - mjm_setString(pchild->classname, childclass.c_str()); + mjm_setString(pchild->childclass, childclass.c_str()); } ReadAttr(elem, "pos", 3, pchild->pos, text); ReadQuat(elem, "quat", pchild->quat, text); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 93c9e91a..c0d72f80 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -807,7 +807,7 @@ string mjXWriter::Write(char *error, size_t error_sz) { Extension(root); Custom(root); Asset(root); - Body(InsertEnd(root, "worldbody"), model->GetWorld()); + Body(InsertEnd(root, "worldbody"), model->GetWorld(), /*frame=*/nullptr); Contact(root); Deformable(root); Equality(root); @@ -1449,13 +1449,19 @@ void mjXWriter::Asset(XMLElement* root) { // recursive body writer -void mjXWriter::Body(XMLElement* elem, mjCBody* body) { +void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) { double unitq[4] = {1, 0, 0, 0}; if (!body) { throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR } + // write frame if classname is defined + if (frame) { + WriteAttrTxt(elem, "name", frame->name); + WriteAttrTxt(elem, "childclass", frame->classname); + } + // write body attributes and inertial if (body!=model->GetWorld()) { WriteAttrTxt(elem, "name", body->name); @@ -1490,26 +1496,31 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { // write joints for (int i=0; ijoints.size(); i++) { + if (body->joints[i]->frame != frame) continue; OneJoint(InsertEnd(elem, "joint"), body->joints[i], body->joints[i]->def); } // write geoms for (int i=0; igeoms.size(); i++) { + if (body->geoms[i]->frame != frame) continue; OneGeom(InsertEnd(elem, "geom"), body->geoms[i], body->geoms[i]->def); } // write sites for (int i=0; isites.size(); i++) { + if (body->sites[i]->frame != frame) continue; OneSite(InsertEnd(elem, "site"), body->sites[i], body->sites[i]->def); } // write cameras for (int i=0; icameras.size(); i++) { + if (body->cameras[i]->frame != frame) continue; OneCamera(InsertEnd(elem, "camera"), body->cameras[i], body->cameras[i]->def); } // write lights for (int i=0; ilights.size(); i++) { + if (body->lights[i]->frame != frame) continue; OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def); } @@ -1518,9 +1529,20 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { OnePlugin(InsertEnd(elem, "plugin"), &body->plugin); } + // write frames + for (int i=0; iframes.size(); i++) { + if (body->frames[i]->frame != frame) continue; + if (!body->frames[i]->name.empty() || !body->frames[i]->classname.empty()) { + Body(InsertEnd(elem, "frame"), body, body->frames[i]); + } else { + Body(elem, body, body->frames[i]); + } + } + // write child bodies recursively for (int i=0; ibodies.size(); i++) { - Body(InsertEnd(elem, "body"), body->bodies[i]); + if (body->bodies[i]->frame != frame) continue; + Body(InsertEnd(elem, "body"), body->bodies[i], nullptr); } } diff --git a/src/xml/xml_native_writer.h b/src/xml/xml_native_writer.h index 16830d38..0a475448 100644 --- a/src/xml/xml_native_writer.h +++ b/src/xml/xml_native_writer.h @@ -40,23 +40,23 @@ class mjXWriter : public mjXBase { mjCModel* model = 0; // XML section writers - void Compiler(tinyxml2::XMLElement* root); // compiler section - void Option(tinyxml2::XMLElement* root); // option section - void Size(tinyxml2::XMLElement* root); // size section - void Visual(tinyxml2::XMLElement* root); // visual section - void Statistic(tinyxml2::XMLElement* root); // statistic section - void Default(tinyxml2::XMLElement* root, mjCDef* def); // default section - void Extension(tinyxml2::XMLElement* root); // extension section - void Custom(tinyxml2::XMLElement* root); // custom section - void Asset(tinyxml2::XMLElement* root); // asset section - void Body(tinyxml2::XMLElement* elem, mjCBody* body); // body/world section - void Contact(tinyxml2::XMLElement* root); // contact section - void Deformable(tinyxml2::XMLElement* root); // deformable section - void Equality(tinyxml2::XMLElement* root); // equality section - void Tendon(tinyxml2::XMLElement* root); // tendon section - void Actuator(tinyxml2::XMLElement* root); // actuator section - void Sensor(tinyxml2::XMLElement* root); // sensor section - void Keyframe(tinyxml2::XMLElement* root); // keyframe section + void Compiler(tinyxml2::XMLElement* root); // compiler section + void Option(tinyxml2::XMLElement* root); // option section + void Size(tinyxml2::XMLElement* root); // size section + void Visual(tinyxml2::XMLElement* root); // visual section + void Statistic(tinyxml2::XMLElement* root); // statistic section + void Default(tinyxml2::XMLElement* root, mjCDef* def); // default section + void Extension(tinyxml2::XMLElement* root); // extension section + void Custom(tinyxml2::XMLElement* root); // custom section + void Asset(tinyxml2::XMLElement* root); // asset section + void Body(tinyxml2::XMLElement* elem, mjCBody* body, mjCFrame* frame); // body/world section + void Contact(tinyxml2::XMLElement* root); // contact section + void Deformable(tinyxml2::XMLElement* root); // deformable section + void Equality(tinyxml2::XMLElement* root); // equality section + void Tendon(tinyxml2::XMLElement* root); // tendon section + void Actuator(tinyxml2::XMLElement* root); // actuator section + void Sensor(tinyxml2::XMLElement* root); // sensor section + void Keyframe(tinyxml2::XMLElement* root); // keyframe section // single element writers, used in defaults and main body void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex); diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 1b37dd4c..2e854d89 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -822,35 +822,72 @@ TEST_F(XMLReaderTest, IncludeAbsoluteTest) { TEST_F(XMLReaderTest, ParseFrame) { static constexpr char xml[] = R"( + + + + + + + + + + + + - + - - - - + + + + + - + - )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_THAT(m->geom_size[ 0], .5); + EXPECT_THAT(m->geom_size[ 3], .6); + EXPECT_THAT(m->geom_size[ 6], .1); + EXPECT_THAT(m->geom_size[ 9], .2); + EXPECT_THAT(m->geom_size[12], .3); mj_deleteModel(m); } +TEST_F(XMLReaderTest, DuplicateFrameName) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, IsNull()) << error.data(); + EXPECT_THAT(error.data(), HasSubstr("repeated name 'frame1'")); +} + // ----------------------- test camera parsing --------------------------------- TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) { diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 2bce5ea3..c9ca0a09 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -724,6 +724,39 @@ TEST_F(XMLWriterTest, WritesActuatorDefaults) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, WritesFrameDefaults) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, NotNull()) << error.data(); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("frame name=\"f1\"")); + EXPECT_THAT(saved_xml, HasSubstr("frame name=\"f2\" childclass=\"dframe\"")); + EXPECT_THAT(saved_xml, Not(HasSubstr(""))); + mj_deleteModel(model); +} + TEST_F(XMLWriterTest, WritesDensity) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c9059bbf..278ed9e5 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -302,6 +302,7 @@ public enum mjtObj : int{ mjOBJ_KEY = 24, mjOBJ_PLUGIN = 25, mjNOBJECT = 26, + mjOBJ_FRAME = 100, } public enum mjtConstraint : int{ mjCNSTR_EQUALITY = 0,