Add childclass support to frames.

PiperOrigin-RevId: 615487831
Change-Id: I0e4d2efd52211701b4286a0b0de84424470a3a47

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
+   unless they specify their own class or another body or frame with a childclass attribute is encountered along the
-   nested bodies. Recall :ref:`CDefault`.
+   chain of nested bodies and frames. Recall :ref:`CDefault`.
 
 .. _body-mocap:
 
@@ -4029,6 +4029,18 @@ Associate this flexcomp with an :ref:`engine plugin<exPlugin>`. 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<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"`

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

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;
 
 

introspect/enums.py

@@ -267,6 +267,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjOBJ_KEY', 24),
              ('mjOBJ_PLUGIN', 25),
              ('mjNOBJECT', 26),
+             ('mjOBJ_FRAME', 100),
          ]),
      )),
     ('mjtConstraint',

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:

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;
   }

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

src/user/user_model.cc

@@ -636,6 +636,7 @@ void mjCModel::MakeLists(mjCBody* body) {
   for (int i=0; i<body->sites.size(); i++) sites.push_back(body->sites[i]);
   for (int i=0; i<body->cameras.size(); i++) cameras.push_back(body->cameras[i]);
   for (int i=0; i<body->lights.size(); i++) lights.push_back(body->lights[i]);
+  for (int i=0; i<body->frames.size(); i++) frames.push_back(body->frames[i]);
 
   // recursive call to all child bodies
   for (int i=0; i<body->bodies.size(); i++) MakeLists(body->bodies[i]);
@@ -2679,18 +2680,20 @@ static void reassignid(vector<T*>& list) {
 template <class T>
 static void processlist(mjListKeyMap& ids, vector<T*>& list,
                         mjtObj type, bool checkrepeat = true) {
-  // loop over list elements
+  // assign ids for regular elements
-  for (size_t i=0; i < list.size(); i++) {
+  if (type < mjNOBJECT) {
-    // check for incompatible id setting; SHOULD NOT OCCUR
+    for (size_t i=0; i < list.size(); i++) {
-    if (list[i]->id!=-1 && list[i]->id!=i) {
+      // check for incompatible id setting; SHOULD NOT OCCUR
-      throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i);
+      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);

src/user/user_model.h

@@ -250,6 +250,7 @@ class mjCModel : private mjSpec {
   std::vector<mjCSite*>   sites;    // list of sites attached to this body
   std::vector<mjCCamera*> cameras;  // list of cameras
   std::vector<mjCLight*>  lights;   // list of lights
+  std::vector<mjCFrame*>  frames;   // list of frames
 
   // array of pointers to each object list (enumerated by type)
   std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists;

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;
 }
 

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);

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);

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; i<body->joints.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; i<body->geoms.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; i<body->sites.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; i<body->cameras.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; i<body->lights.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; i<body->frames.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; i<body->bodies.size(); i++) {
-    Body(InsertEnd(elem, "body"), body->bodies[i]);
+    if (body->bodies[i]->frame != frame) continue;
+    Body(InsertEnd(elem, "body"), body->bodies[i], nullptr);
   }
 }
 

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 Compiler(tinyxml2::XMLElement* root);                              // compiler section
-  void Option(tinyxml2::XMLElement* root);                      // option section
+  void Option(tinyxml2::XMLElement* root);                                // option section
-  void Size(tinyxml2::XMLElement* root);                        // size section
+  void Size(tinyxml2::XMLElement* root);                                  // size section
-  void Visual(tinyxml2::XMLElement* root);                      // visual section
+  void Visual(tinyxml2::XMLElement* root);                                // visual section
-  void Statistic(tinyxml2::XMLElement* root);                   // statistic section
+  void Statistic(tinyxml2::XMLElement* root);                             // statistic section
-  void Default(tinyxml2::XMLElement* root, mjCDef* def);        // default section
+  void Default(tinyxml2::XMLElement* root, mjCDef* def);                  // default section
-  void Extension(tinyxml2::XMLElement* root);                   // extension section
+  void Extension(tinyxml2::XMLElement* root);                             // extension section
-  void Custom(tinyxml2::XMLElement* root);                      // custom section
+  void Custom(tinyxml2::XMLElement* root);                                // custom section
-  void Asset(tinyxml2::XMLElement* root);                       // asset section
+  void Asset(tinyxml2::XMLElement* root);                                 // asset section
-  void Body(tinyxml2::XMLElement* elem, mjCBody* body);         // body/world section
+  void Body(tinyxml2::XMLElement* elem, mjCBody* body, mjCFrame* frame);  // body/world section
-  void Contact(tinyxml2::XMLElement* root);                     // contact section
+  void Contact(tinyxml2::XMLElement* root);                               // contact section
-  void Deformable(tinyxml2::XMLElement* root);                  // deformable section
+  void Deformable(tinyxml2::XMLElement* root);                            // deformable section
-  void Equality(tinyxml2::XMLElement* root);                    // equality section
+  void Equality(tinyxml2::XMLElement* root);                              // equality section
-  void Tendon(tinyxml2::XMLElement* root);                      // tendon section
+  void Tendon(tinyxml2::XMLElement* root);                                // tendon section
-  void Actuator(tinyxml2::XMLElement* root);                    // actuator section
+  void Actuator(tinyxml2::XMLElement* root);                              // actuator section
-  void Sensor(tinyxml2::XMLElement* root);                      // sensor section
+  void Sensor(tinyxml2::XMLElement* root);                                // sensor section
-  void Keyframe(tinyxml2::XMLElement* root);                    // keyframe section
+  void Keyframe(tinyxml2::XMLElement* root);                              // keyframe section
 
   // single element writers, used in defaults and main body
   void OneFlex(tinyxml2::XMLElement* elem,      mjCFlex* pflex);

test/xml/xml_native_reader_test.cc

@@ -822,35 +822,72 @@ TEST_F(XMLReaderTest, IncludeAbsoluteTest) {
 TEST_F(XMLReaderTest, ParseFrame) {
   static constexpr char xml[] = R"(
   <mujoco>
+    <default>
+      <default class="frame">
+        <geom size=".1"/>
+      </default>
+      <default class="body">
+        <geom size=".2"/>
+      </default>
+      <default class="geom">
+        <geom size=".3"/>
+      </default>
+    </default>
+
     <worldbody>
       <frame euler="0 0 30">
-        <geom size=".1" euler="0 0 20"/>
+        <geom size=".5" euler="0 0 20"/>
       </frame>
 
       <body>
-        <frame pos="0 1 0">
+        <frame pos="0 1 0" childclass="frame">
-          <geom size=".1" pos="0 1 0"/>
+          <geom pos="0 1 0"/>
-          <body pos="1 0 0">
+          <body pos="1 0 0" childclass="body">
-            <geom size=".1" pos="0 0 1"/>
+            <geom pos="0 0 1"/>
+            <geom pos="0 0 1" class="geom"/>
           </body>
         </frame>
       </body>
 
       <frame euler="0 0 30">
         <frame euler="0 0 20">
-          <geom size=".1"/>
+          <geom size=".6"/>
         </frame>
       </frame>
     </worldbody>
   </mujoco>
-
   )";
   std::array<char, 1024> 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"(
+  <mujoco>
+    <worldbody>
+      <frame name="frame1" euler="0 0 30">
+        <geom size=".1"/>
+      </frame>
+
+      <frame name="frame1" euler="0 0 30">
+        <geom size=".1"/>
+      </frame>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> 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) {

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"(
+  <mujoco>
+    <default class="dframe">
+      <geom size=".1"/>
+    </default>
+
+    <worldbody>
+      <frame name="f1" euler="0 0 30">
+        <geom size=".5" euler="0 0 20"/>
+      </frame>
+
+      <body>
+        <frame pos="0 1 0" name="f2" childclass="dframe">
+          <geom pos="0 1 0"/>
+          <body pos="1 0 0">
+            <geom pos="0 0 1"/>
+          </body>
+        </frame>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> 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("<frame>")));
+  mj_deleteModel(model);
+}
+
 TEST_F(XMLWriterTest, WritesDensity) {
   static constexpr char xml[] = R"(
   <mujoco>

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,