Add replicate element to MJCF. Fixes #553

PiperOrigin-RevId: 629393578
Change-Id: Ia1db1b9e47eba8868117b0f7b85edcd0cc1b6d79

src/user/user_api.cc

@@ -111,10 +111,11 @@ const char* mjs_getError(mjSpec* s) {
 
 
 // Detach body from mjSpec, return 0 if success.
-int mjs_detachBody(mjSpec* s, const mjsBody* b) {
+int mjs_detachBody(mjSpec* s, mjsBody* b) {
   mjCModel* model = static_cast<mjCModel*>(s->element);
   mjCBody* body = static_cast<mjCBody*>(b->element);
   *model -= *body;
+  mjs_deleteBody(b);
   return 0;
 }
 

src/user/user_api.h

@@ -778,8 +778,8 @@ MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
 MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
                           const char* prefix, const char* suffix);
 
-// Detach body from mjSpec, remove all references, return 0 on success.
-MJAPI int mjs_detachBody(mjSpec* s, const mjsBody* b);
+// Detach body from mjSpec, remove all references and delete the body, return 0 on success.
+MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b);
 
 
 //---------------------------------- Add tree elements ---------------------------------------------

src/user/user_model.cc

@@ -230,21 +230,27 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
   // create global lists
   MakeLists(bodies[0]);
   CreateObjectLists();
-  ProcessLists();
+  ProcessLists(/*checkrepeat=*/false);
 
   // copy all elements not in the tree
   std::map<mjCDef*, int> def_map;
   int ndefaults = (int)other.defaults.size();
   for (int i = 0; i < ndefaults; i++) {
-    defaults.push_back(new mjCDef(*other.defaults[i]));
+    if (this != &other) {
+      defaults.push_back(new mjCDef(*other.defaults[i]));
+    }
     def_map[other.defaults[i]] = i;
   }
+  if (this != &other) {
+    // do not copy assets for self-attach
+    CopyList(meshes, other.meshes, def_map, defaults);
+    CopyList(skins, other.skins, def_map, defaults);
+    CopyList(hfields, other.hfields, def_map, defaults);
+    CopyList(textures, other.textures, def_map, defaults);
+    CopyList(materials, other.materials, def_map, defaults);
+    CopyList(keys, other.keys, def_map, defaults);
+  }
   CopyList(flexes, other.flexes, def_map, defaults);
-  CopyList(meshes, other.meshes, def_map, defaults);
-  CopyList(skins, other.skins, def_map, defaults);
-  CopyList(hfields, other.hfields, def_map, defaults);
-  CopyList(textures, other.textures, def_map, defaults);
-  CopyList(materials, other.materials, def_map, defaults);
   CopyList(pairs, other.pairs, def_map, defaults);
   CopyList(excludes, other.excludes, def_map, defaults);
   CopyList(tendons, other.tendons, def_map, defaults);
@@ -254,7 +260,6 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
   CopyList(numerics, other.numerics, def_map, defaults);
   CopyList(texts, other.texts, def_map, defaults);
   CopyList(tuples, other.tuples, def_map, defaults);
-  CopyList(keys, other.keys, def_map, defaults);
 
   // plugins are global
   plugins = other.plugins;
@@ -309,6 +314,7 @@ void mjCModel::RemoveFromList(std::vector<T*>& list, const mjCModel& other) {
     element->id -= removed;
     try {
       // check if the element contains an error
+      element->NameSpace(&other);
       element->CopyFromSpec();
       element->ResolveReferences(&other);
     } catch (mjCError err) {
@@ -316,6 +322,7 @@ void mjCModel::RemoveFromList(std::vector<T*>& list, const mjCModel& other) {
     }
     try {
       // check if the element references something that was removed
+      element->NameSpace(this);
       element->ResolveReferences(this);
     } catch (mjCError err) {
       delete element;
@@ -333,7 +340,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
   mjCModel oldmodel(*this);
   oldmodel.MakeLists(oldmodel.bodies[0]);
   oldmodel.CreateObjectLists();
-  oldmodel.ProcessLists();
+  oldmodel.ProcessLists(/*checkrepeat=*/false);
 
   // remove body from tree
   *bodies[0] -= subtree;
@@ -341,7 +348,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
   // create global lists
   MakeLists(bodies[0]);
   CreateObjectLists();
-  ProcessLists();
+  ProcessLists(/*checkrepeat=*/false);
 
   // check if we have to remove anything else
   RemoveFromList(pairs, oldmodel);
@@ -3044,15 +3051,16 @@ static void processlist(mjListKeyMap& ids, vector<T*>& list,
 
 
 // set object ids, check for repeated names
-void mjCModel::ProcessLists() {
+void mjCModel::ProcessLists(bool checkrepeat) {
   for (int i = 0; i < mjNOBJECT; i++) {
     if (i != mjOBJ_XBODY && object_lists[i]) {
-      processlist(ids, *object_lists[i], (mjtObj) i);
+      ids[i].clear();
+      processlist(ids, *object_lists[i], (mjtObj) i, checkrepeat);
     }
   }
 
   // check repeated names in meta elements
-  processlist(ids, frames, mjOBJ_FRAME);
+  processlist(ids, frames, mjOBJ_FRAME, checkrepeat);
 }
 
 

src/user/user_model.h

@@ -307,7 +307,7 @@ class mjCModel : public mjCModel_, private mjSpec {
   void CreateObjectLists();
 
   // populate objects ids
-  void ProcessLists();
+  void ProcessLists(bool checkrepeat = true);
 
   mjListKeyMap ids;   // map from object names to ids
   mjCError errInfo;   // last error info

src/user/user_objects.cc

@@ -728,6 +728,9 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
     lights.clear();
     id = other.id;
 
+    // copy defaults
+    def = other.def;
+
     // add elements to lists
     *this += other;
   }
@@ -797,18 +800,17 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) {
   CopyList(cameras, subtree->cameras, fmap, &other);
   CopyList(lights, subtree->lights, fmap, &other);
 
-  for (int i=0; i<subtree->bodies.size(); i++) {
+  int nbodies = (int)subtree->bodies.size();
+  for (int i=0; i<nbodies; i++) {
     if (!other.IsAncestor(subtree->bodies[i]->frame)) {
       continue;
     }
     bodies.push_back(new mjCBody(*subtree->bodies[i], model));  // triggers recursive call
     bodies.back()->frame =
         subtree->bodies[i]->frame ? frames[fmap[subtree->bodies[i]->frame]] : nullptr;
+    bodies.back()->NameSpace_(other.model, /*propagate=*/ false);
   }
 
-  // name space
-  this->NameSpace(other.model);
-
   // attach referencing elements
   *model += *other.model;
 
@@ -832,14 +834,14 @@ void mjCBody::CopyList(std::vector<T*>& dst, const std::vector<T*>& src,
     dst.push_back(new T(*src[i]));
     dst.back()->body = this;
     dst.back()->model = model;
+    dst.back()->id = -1;
+    dst.back()->def = src[i]->def;
 
     // assign dst frame to src frame
     dst.back()->frame = src[i]->frame ? frames[fmap[src[i]->frame]] : nullptr;
 
-    // set namespace from pframe if given
-    if (pframe) {
-      dst.back()->NameSpace(pframe->model);
-    }
+    // set namespace
+    dst.back()->NameSpace(src[i]->model);
   }
 }
 
@@ -906,8 +908,25 @@ mjCBody::~mjCBody() {
 
 // apply prefix and suffix, propagate to children
 void mjCBody::NameSpace(const mjCModel* m) {
+  NameSpace_(m, true);
+}
+
+
+
+// apply prefix and suffix, propagate to all descendents or only to child bodies
+void mjCBody::NameSpace_(const mjCModel* m, bool propagate) {
   if (!name.empty()) {
-    name = prefix + name + suffix;
+    name = m->prefix + name + m->suffix;
+  }
+
+  for (auto& body : bodies) {
+    body->prefix = m->prefix;
+    body->suffix = m->suffix;
+    body->NameSpace_(m, propagate);
+  }
+
+  if (!propagate) {
+    return;
   }
 
   for (auto& joint : joints) {
@@ -929,12 +948,6 @@ void mjCBody::NameSpace(const mjCModel* m) {
   for (auto& light : lights) {
     light->NameSpace(m);
   }
-
-  for (auto& body : bodies) {
-    body->prefix = m->prefix;
-    body->suffix = m->suffix;
-    body->NameSpace(m);
-  }
 }
 
 
@@ -1836,6 +1849,23 @@ void mjCGeom::CopyFromSpec() {
 
 
 
+void mjCGeom::NameSpace(const mjCModel* m) {
+  if (!name.empty()) {
+    name = m->prefix + name + m->suffix;
+  }
+  if (!spec_material_.empty() && model != m) {
+    spec_material_ = m->prefix + spec_material_ + m->suffix;
+  }
+  if (!spec_hfieldname_.empty() && model != m) {
+    spec_hfieldname_ = m->prefix + spec_hfieldname_ + m->suffix;
+  }
+  if (!spec_meshname_.empty() && model != m) {
+    spec_meshname_ = m->prefix + spec_meshname_ + m->suffix;
+  }
+}
+
+
+
 // compute geom volume
 double mjCGeom::GetVolume(void) {
   double height;
@@ -2196,7 +2226,7 @@ void mjCGeom::Compile(void) {
   }
 
   // check hfield
-  if ((type==mjGEOM_HFIELD && !hfield) || (type!=mjGEOM_HFIELD && hfield)) {
+  if ((type==mjGEOM_HFIELD && !hfield) || (type != mjGEOM_HFIELD && hfield)) {
     throw mjCError(this, "hfield geom '%s' (id = %d) must have valid hfieldid", name.c_str(), id);
   }
 
@@ -2272,7 +2302,7 @@ void mjCGeom::Compile(void) {
 
     // fit geom if type is not mjGEOM_MESH
     double meshpos[3];
-    if (type!=mjGEOM_MESH && type!=mjGEOM_SDF) {
+    if (type != mjGEOM_MESH && type != mjGEOM_SDF) {
       mesh->FitGeom(this, meshpos);
 
       // remove reference to mesh
@@ -2578,7 +2608,9 @@ void mjCCamera::NameSpace(const mjCModel* m) {
   if (!name.empty()) {
     name = m->prefix + name + m->suffix;
   }
-  spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
+  if (!spec_targetbody_.empty()) {
+    spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
+  }
 }
 
 
@@ -2726,7 +2758,9 @@ void mjCLight::NameSpace(const mjCModel* m) {
   if (!name.empty()) {
     name = m->prefix + name + m->suffix;
   }
-  spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
+  if (!spec_targetbody_.empty()) {
+    spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
+  }
 }
 
 
@@ -3515,7 +3549,7 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
   LoadFlip(filename, vfs, image, w, h);
 
   // check gridsize for compatibility
-  if (w/gridsize[1]!=h/gridsize[0] || (w%gridsize[1]) || (h%gridsize[0])) {
+  if (w/gridsize[1] != h/gridsize[0] || (w%gridsize[1]) || (h%gridsize[0])) {
     throw mjCError(this,
                    "PNG size must be integer multiple of gridsize in texture '%s' (id %d)",
                    (const char*)file_.c_str(), id);
@@ -3621,7 +3655,7 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
       LoadFlip(filename, vfs, image, w, h);
 
       // PNG must be square
-      if (w!=h) {
+      if (w != h) {
         throw mjCError(this,
                        "Non-square PNG file '%s' in cube or skybox id %d",
                        (const char*)cubefiles_[i].c_str(), id);
@@ -3638,7 +3672,7 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
       }
 
       // otherwise check size
-      else if (width!=w) {
+      else if (width != w) {
         throw mjCError(this,
                        "PNG file '%s' has incompatible size in texture id %d",
                        (const char*)cubefiles_[i].c_str(), id);
@@ -3674,14 +3708,14 @@ void mjCTexture::Compile(const mjVFS* vfs) {
   CopyFromSpec();
 
   // builtin
-  if (builtin!=mjBUILTIN_NONE) {
+  if (builtin != mjBUILTIN_NONE) {
     // check size
     if (width<1 || height<1) {
       throw mjCError(this, "Invalid width or height of builtin texture");
     }
 
     // adjust height of cube texture
-    if (type!=mjTEXTURE_2D) {
+    if (type != mjTEXTURE_2D) {
       height = 6*width;
     }
 
@@ -3815,6 +3849,17 @@ void mjCMaterial::CopyFromSpec() {
 
 
 
+void mjCMaterial::NameSpace(const mjCModel* m) {
+  if (!name.empty()) {
+    name = m->prefix + name + m->suffix;
+  }
+  if (!spec_texture_.empty() && model != m) {
+    spec_texture_ = m->prefix + spec_texture_ + m->suffix;
+  }
+}
+
+
+
 // compiler
 void mjCMaterial::Compile(void) {
   CopyFromSpec();
@@ -3891,8 +3936,8 @@ void mjCPair::NameSpace(const mjCModel* m) {
   if (!name.empty()) {
     name = m->prefix + name + m->suffix;
   }
-  spec_geomname1_ = m->prefix + spec_geomname1_ + m->suffix;
-  spec_geomname2_ = m->prefix + spec_geomname2_ + m->suffix;
+  prefix = m->prefix;
+  suffix = m->suffix;
 }
 
 
@@ -3908,18 +3953,32 @@ void mjCPair::CopyFromSpec() {
 
 
 void mjCPair::ResolveReferences(const mjCModel* m) {
-  // find geom 1
+  geomname1_ = prefix + geomname1_ + suffix;
+  geomname2_ = prefix + geomname2_ + suffix;
   geom1 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname1_);
+  geom2 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname2_);
+
+  if (!geom1 && geom2) {
+    geomname1_ = spec_geomname1_;
+    geom1 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname1_);
+  }
+  if (geom1 && !geom2) {
+    geomname2_ = spec_geomname2_;
+    geom2 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname2_);
+  }
+
   if (!geom1) {
     throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id);
   }
-
-  // find geom 2
-  geom2 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname2_);
   if (!geom2) {
     throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id);
   }
 
+  spec_geomname1_ = geomname1_;
+  spec_geomname2_ = geomname2_;
+  prefix.clear();
+  suffix.clear();
+
   // swap if body1 > body2
   if (geom1->body->id > geom2->body->id) {
     string nametmp = geomname1_;
@@ -3964,7 +4023,7 @@ void mjCPair::Compile(void) {
   }
 
   // set undefined condim, friction, solref, solimp: different priority
-  if (geom1->priority!=geom2->priority) {
+  if (geom1->priority != geom2->priority) {
     mjCGeom* pgh = (geom1->priority>geom2->priority ? geom1 : geom2);
 
     // condim
@@ -4101,8 +4160,8 @@ void mjCBodyPair::NameSpace(const mjCModel* m) {
   if (!name.empty()) {
     name = m->prefix + name + m->suffix;
   }
-  spec_bodyname1_ = m->prefix + spec_bodyname1_ + m->suffix;
-  spec_bodyname2_ = m->prefix + spec_bodyname2_ + m->suffix;
+  prefix = m->prefix;
+  suffix = m->suffix;
 }
 
 
@@ -4118,18 +4177,32 @@ void mjCBodyPair::CopyFromSpec() {
 
 
 void mjCBodyPair::ResolveReferences(const mjCModel* m) {
-  // find body 1
+  bodyname1_ = prefix + bodyname1_ + suffix;
+  bodyname2_ = prefix + bodyname2_ + suffix;
   mjCBody* pb1 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname1_);
+  mjCBody* pb2 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname2_);
+
+  if (!pb1 && pb2) {
+    bodyname1_ = spec_bodyname1_;
+    pb1 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname1_);
+  }
+  if (pb1 && !pb2) {
+    bodyname2_ = spec_bodyname2_;
+    pb2 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname2_);
+  }
+
   if (!pb1) {
     throw mjCError(this, "body '%s' not found in bodypair %d", bodyname1_.c_str(), id);
   }
-
-  // find body 2
-  mjCBody* pb2 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname2_);
   if (!pb2) {
     throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2_.c_str(), id);
   }
 
+  spec_bodyname1_ = bodyname1_;
+  spec_bodyname2_ = bodyname2_;
+  prefix.clear();
+  suffix.clear();
+
   // swap if body1 > body2
   if (pb1->id > pb2->id) {
     string nametmp = bodyname1_;
@@ -4391,9 +4464,8 @@ void mjCTendon::NameSpace(const mjCModel* m) {
   if (!name.empty()) {
     name = m->prefix + name + m->suffix;
   }
-  for (int i=0; i<path.size(); i++) {
-    path[i]->NameSpace(m);
-  }
+  prefix = m->prefix;
+  suffix = m->suffix;
 }
 
 
@@ -4517,9 +4589,28 @@ mjCWrap* mjCTendon::GetWrap(int id) {
 
 
 void mjCTendon::ResolveReferences(const mjCModel* m) {
+  int nfailure = 0;
   for (int i=0; i<path.size(); i++) {
-    path[i]->ResolveReferences(m);
+    std::string pname = path[i]->name;
+    std::string psidesite = path[i]->sidesite;
+    try {
+      // look for wrapped element with namespace
+      path[i]->name = prefix + pname + suffix;
+      path[i]->sidesite = prefix + psidesite + suffix;
+      path[i]->ResolveReferences(m);
+    } catch(mjCError) {
+      // remove namespace from wrap names
+      path[i]->name = pname;
+      path[i]->sidesite = psidesite;
+      path[i]->ResolveReferences(m);
+      nfailure++;
+    }
   }
+  if (nfailure==path.size()) {
+    throw mjCError(this, "tendon '%s' (id = %d): no attached reference found", name.c_str(), id);
+  }
+  prefix.clear();
+  suffix.clear();
 }
 
 
@@ -4543,7 +4634,7 @@ void mjCTendon::Compile(void) {
   }
 
   // determine type
-  bool spatial = (path[0]->type!=mjWRAP_JOINT);
+  bool spatial = (path[0]->type != mjWRAP_JOINT);
 
   // require at least two objects in spatial path
   if (spatial && sz<2) {
@@ -4564,7 +4655,7 @@ void mjCTendon::Compile(void) {
     // fixed
     if (!spatial) {
       // make sure all objects are joints
-      if (path[i]->type!=mjWRAP_JOINT) {
+      if (path[i]->type != mjWRAP_JOINT) {
         throw mjCError(this, "tendon '%s' (id = %d): spatial object found in fixed path at pos %d",
                        name.c_str(), id, i);
       }
@@ -4607,7 +4698,7 @@ void mjCTendon::Compile(void) {
       case mjWRAP_SPHERE:
       case mjWRAP_CYLINDER:
         // geom must be bracketed by sites
-        if (i==0 || i==sz-1 || path[i-1]->type!=mjWRAP_SITE || path[i+1]->type!=mjWRAP_SITE) {
+        if (i==0 || i==sz-1 || path[i-1]->type != mjWRAP_SITE || path[i+1]->type != mjWRAP_SITE) {
           throw mjCError(this,
                          "tendon '%s' (id = %d): geom at pos %d not bracketed by sites",
                          name.c_str(), id, i);
@@ -5187,8 +5278,8 @@ void mjCSensor::NameSpace(const mjCModel* m) {
   if (!name.empty()) {
     name = m->prefix + name + m->suffix;
   }
-  spec_objname_ = m->prefix + spec_objname_ + m->suffix;
-  spec_refname_ = m->prefix + spec_refname_ + m->suffix;
+  prefix = m->prefix;
+  suffix = m->suffix;
 }
 
 
@@ -5210,15 +5301,35 @@ void mjCSensor::CopyFromSpec() {
 
 
 void mjCSensor::ResolveReferences(const mjCModel* m) {
+  objname_ = prefix + objname_ + suffix;
+  refname_ = prefix + refname_ + suffix;
+
+  // get references using the namespace
+  if (objtype != mjOBJ_UNKNOWN) {
+    obj = m->FindObject(objtype, objname_);
+  }
+  if (reftype != mjOBJ_UNKNOWN) {
+    ref = m->FindObject(reftype, refname_);
+  }
+
+  // if failure and both were requested, use namespace only on one
+  if (objtype != mjOBJ_UNKNOWN && reftype != mjOBJ_UNKNOWN && !obj && ref) {
+    objname_ = spec_objname_;
+    obj = m->FindObject(objtype, objname_);
+  }
+  if (objtype != mjOBJ_UNKNOWN && reftype != mjOBJ_UNKNOWN && obj && !ref) {
+    refname_ = spec_refname_;
+    ref = m->FindObject(reftype, refname_);
+  }
+
   // get objid from objtype and objname
-  if (objtype!=mjOBJ_UNKNOWN) {
+  if (objtype != mjOBJ_UNKNOWN) {
     // check for missing object name
     if (objname_.empty()) {
       throw mjCError(this, "missing name of sensorized object in sensor");
     }
 
     // find name
-    obj = m->FindObject(objtype, objname_);
     if (!obj) {
       throw mjCError(this, "unrecognized name '%s' of sensorized object", objname_.c_str());
     }
@@ -5234,21 +5345,20 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
   }
 
   // get refid from reftype and refname
-  if (reftype!=mjOBJ_UNKNOWN) {
+  if (reftype != mjOBJ_UNKNOWN) {
     // check for missing object name
     if (refname_.empty()) {
       throw mjCError(this, "missing name of reference frame object in sensor");
     }
 
     // find name
-    ref = m->FindObject(reftype, refname_);
     if (!ref) {
       throw mjCError(this, "unrecognized name '%s' of reference frame object", refname_.c_str());
     }
 
     // must be attached to object with spatial frame
-    if (reftype!=mjOBJ_BODY && reftype!=mjOBJ_XBODY &&
-        reftype!=mjOBJ_GEOM && reftype!=mjOBJ_SITE && reftype!=mjOBJ_CAMERA) {
+    if (reftype != mjOBJ_BODY && reftype != mjOBJ_XBODY &&
+        reftype != mjOBJ_GEOM && reftype != mjOBJ_SITE && reftype != mjOBJ_CAMERA) {
       throw mjCError(this,
                      "reference frame object must be (x)body, geom, site or camera in sensor");
     }
@@ -5256,6 +5366,11 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
     // get sensorized object id
     refid = ref->id;
   }
+
+  spec_objname_ = objname_;
+  spec_refname_ = refname_;
+  prefix.clear();
+  suffix.clear();
 }
 
 
@@ -5537,7 +5652,7 @@ void mjCSensor::Compile(void) {
     }
 
     // make sure dim is consistent with datatype
-    if (datatype==mjDATATYPE_AXIS && dim!=3) {
+    if (datatype==mjDATATYPE_AXIS && dim != 3) {
       throw mjCError(this,
                      "datatype AXIS requires dim=3 in sensor");
     }
@@ -5843,7 +5958,7 @@ void mjCTuple::ResolveReferences(const mjCModel* m) {
   }
 
   // check for size conflict
-  if (objtype_.size()!=objname_.size() || objtype_.size()!=objprm_.size()) {
+  if (objtype_.size() != objname_.size() || objtype_.size() != objprm_.size()) {
     throw mjCError(this,
                    "tuple '%s' (id = %d) has object arrays with different sizes", name.c_str(), id);
   }
@@ -5984,7 +6099,7 @@ void mjCKey::Compile(const mjModel* m) {
     for (int i=0; i<m->nq; i++) {
       qpos_[i] = (double)m->qpos0[i];
     }
-  } else if (qpos_.size()!=m->nq) {
+  } else if (qpos_.size() != m->nq) {
     throw mjCError(this, "key %d: invalid qpos size, expected length %d", nullptr, id, m->nq);
   }
 
@@ -5994,7 +6109,7 @@ void mjCKey::Compile(const mjModel* m) {
     for (int i=0; i<m->nv; i++) {
       qvel_[i] = 0;
     }
-  } else if (qvel_.size()!=m->nv) {
+  } else if (qvel_.size() != m->nv) {
     throw mjCError(this, "key %d: invalid qvel size, expected length %d", nullptr, id, m->nv);
   }
 
@@ -6004,7 +6119,7 @@ void mjCKey::Compile(const mjModel* m) {
     for (int i=0; i<m->na; i++) {
       act_[i] = 0;
     }
-  } else if (act_.size()!=m->na) {
+  } else if (act_.size() != m->na) {
     throw mjCError(this, "key %d: invalid act size, expected length %d", nullptr, id, m->na);
   }
 
@@ -6021,7 +6136,7 @@ void mjCKey::Compile(const mjModel* m) {
         }
       }
     }
-  } else if (mpos_.size()!=3*m->nmocap) {
+  } else if (mpos_.size() != 3*m->nmocap) {
     throw mjCError(this, "key %d: invalid mpos size, expected length %d", nullptr, id, 3*m->nmocap);
   }
 
@@ -6039,7 +6154,7 @@ void mjCKey::Compile(const mjModel* m) {
         }
       }
     }
-  } else if (mquat_.size()!=4*m->nmocap) {
+  } else if (mquat_.size() != 4*m->nmocap) {
     throw mjCError(this, "key %d: invalid mquat size, expected length %d", nullptr, id, 4*m->nmocap);
   }
 
@@ -6049,7 +6164,7 @@ void mjCKey::Compile(const mjModel* m) {
     for (int i=0; i<m->nu; i++) {
       ctrl_[i] = 0;
     }
-  } else if (ctrl_.size()!=m->nu) {
+  } else if (ctrl_.size() != m->nu) {
     throw mjCError(this, "key %d: invalid ctrl size, expected length %d", nullptr, id, m->nu);
   }
 }

src/user/user_objects.h

@@ -322,6 +322,7 @@ class mjCBody : public mjCBody_, private mjsBody {
 
   void CopyFromSpec();                 // copy spec into attributes
   void PointToLocal(void);
+  void NameSpace_(const mjCModel* m, bool propagate = true);
 
   // copy src list of elements into dst; set body, model and frame
   template <typename T>
@@ -496,6 +497,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
   void ComputeAABB(void);             // compute axis-aligned bounding box
   void CopyFromSpec(void);
   void PointToLocal(void);
+  void NameSpace(const mjCModel* m);
 
   // inherited
   using mjCBase::classname;
@@ -1080,6 +1082,7 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial {
 
   void CopyFromSpec();
   void PointToLocal();
+  void NameSpace(const mjCModel* m);
 
   std::string get_texture() { return texture_; }
   void del_texture() { texture_.clear(); }

src/xml/xml_native_reader.cc

@@ -74,6 +74,16 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjsPlugin* p) {
     mjs_setPluginAttributes(p, &config_attribs);
   }
 }
+
+static void UpdateString(std::string& psuffix, int count, int i) {
+  int ndigits = std::to_string(count).length();
+  std::string i_string = std::to_string(i);
+  std::string prefix = "";
+  while (ndigits-- > i_string.length()) {
+    prefix += '0';
+  }
+  psuffix += prefix + i_string;
+}
 }  // namespace
 
 
@@ -870,11 +880,6 @@ void mjXReader::Parse(XMLElement* root) {
     Asset(section);
   }
 
-  for (XMLElement* section = FirstChildElement(root, "worldbody"); section;
-       section = NextSiblingElement(section, "worldbody")) {
-    Body(section, mjs_findBody(model, "world"), nullptr);
-  }
-
   for (XMLElement* section = FirstChildElement(root, "contact"); section;
        section = NextSiblingElement(section, "contact")) {
     Contact(section);
@@ -909,6 +914,11 @@ void mjXReader::Parse(XMLElement* root) {
        section = NextSiblingElement(section, "keyframe")) {
     Keyframe(section);
   }
+
+  for (XMLElement* section = FirstChildElement(root, "worldbody"); section;
+       section = NextSiblingElement(section, "worldbody")) {
+    Body(section, mjs_findBody(model, "world"), nullptr);
+  }
 }
 
 
@@ -3371,6 +3381,72 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) {
       Body(elem, pbody, pframe);
     }
 
+    // replicate sub-element
+    else if (name=="replicate") {
+      int count;
+      double offset[3] = {0, 0, 0};
+      double euler[3] = {0, 0, 0};
+      std::string separator = "";
+      ReadAttr(elem, "count", 1, &count, text, true);
+      ReadAttr(elem, "offset", 3, offset, text);
+      ReadAttr(elem, "euler", 3, euler, text);
+      ReadAttrTxt(elem, "sep", separator);
+
+      // store rotation difference
+      mjsOrientation alt;
+      mjs_defaultOrientation(alt);
+      alt.type = mjORIENTATION_EULER;
+      mjuu_copyvec(alt.euler, euler, 3);
+      double rotation[4] = {1, 0, 0, 0};
+      mjs_resolveOrientation(rotation, model->degree, model->euler, &alt);
+
+      // read childdef
+      mjsDefault* childdef = 0;
+      if (ReadAttrTxt(elem, "childclass", text)) {
+        childdef = mjs_findDefault(model, text.c_str());
+        mjs_findDefault(model, text.c_str());
+        if (!childdef) {
+          throw mjXError(elem, "unknown default childclass");
+        }
+      }
+
+      // create subtree
+      mjsBody* subtree = mjs_addBody(pbody, childdef);
+      double pos[3] = {0, 0, 0};
+      double quat[4] = {1, 0, 0, 0};
+
+      for (int i = 0; i < count; i++) {
+        // create parent frame
+        mjsFrame* pframe = mjs_addFrame(subtree, frame);
+        mjs_setString(pframe->info, ("line = " + std::to_string(elem->GetLineNum())).c_str());
+        mjs_setDefault(pframe->element, childdef ? childdef : def);
+
+        // accumulate rotation
+        mjuu_setvec(pframe->pos, pos[0], pos[1], pos[2]);
+        mjuu_frameaccum(pos, quat, offset, rotation);
+
+        // overwrite orientation to increase precision
+        alt.euler[0] = i*euler[0];
+        alt.euler[1] = i*euler[1];
+        alt.euler[2] = i*euler[2];
+        mjs_resolveOrientation(quat, model->degree, model->euler, &alt);
+        mjuu_setvec(pframe->quat, quat[0], quat[1], quat[2], quat[3]);
+
+        // process suffix
+        std::string suffix = separator;
+        UpdateString(suffix, count, i);
+
+        // process subtree
+        Body(elem, subtree, pframe);
+
+        // attach to parent
+        mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str());
+      }
+
+      // delete subtree
+      mjs_detachBody(model, subtree);
+    }
+
     // body sub-element
     else if (name=="body") {
       // read childdef

src/xml/xml_native_writer.cc

@@ -811,7 +811,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
   Extension(root);
   Custom(root);
   Asset(root);
-  Body(InsertEnd(root, "worldbody"), model->GetWorld(), /*frame=*/nullptr);
+  Body(InsertEnd(root, "worldbody"), model->GetWorld());
   Contact(root);
   Deformable(root);
   Equality(root);
@@ -1456,20 +1456,31 @@ void mjXWriter::Asset(XMLElement* root) {
 
 
 
+XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) {
+  if (!frame) {
+    return elem;
+  }
+
+  if (frame->name.empty() && frame->classname.empty()) {
+    return elem;
+  }
+
+  XMLElement* frame_elem = InsertEnd(elem, "frame");
+  WriteAttrTxt(frame_elem, "name", frame->name);
+  WriteAttrTxt(frame_elem, "childclass", frame->classname);
+  return frame_elem;
+}
+
+
+
 // recursive body and frame writer
-void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
+void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
   double unitq[4] = {1, 0, 0, 0};
 
   if (!body) {
     throw mjXError(0, "missing body in XML write");  // SHOULD NOT OCCUR
   }
 
-  // write frame if defined
-  if (frame) {
-    WriteAttrTxt(elem, "name", frame->name);
-    WriteAttrTxt(elem, "childclass", frame->classname);
-  }
-
   // write body attributes and inertial
   else if (body!=model->GetWorld()) {
     WriteAttrTxt(elem, "name", body->name);
@@ -1504,32 +1515,32 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
 
   // 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);
+    XMLElement* celem = OneFrame(elem, body->joints[i]->frame);
+    OneJoint(InsertEnd(celem, "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);
+    XMLElement* celem = OneFrame(elem, body->geoms[i]->frame);
+    OneGeom(InsertEnd(celem, "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);
+    XMLElement* celem = OneFrame(elem, body->sites[i]->frame);
+    OneSite(InsertEnd(celem, "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);
+    XMLElement* celem = OneFrame(elem, body->cameras[i]->frame);
+    OneCamera(InsertEnd(celem, "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);
+    XMLElement* celem = OneFrame(elem, body->lights[i]->frame);
+    OneLight(InsertEnd(celem, "light"), body->lights[i], body->lights[i]->def);
   }
 
   // write plugin
@@ -1537,28 +1548,10 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
     OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
   }
 
-  // write frames
-  for (int i=0; i<body->frames.size(); i++) {
-    // skip current frame
-    if (body->frames[i]->frame != frame) {
-      continue;
-    }
-
-    // write frame if named or has defaults
-    if (!body->frames[i]->name.empty() || !body->frames[i]->classname.empty()) {
-      Body(InsertEnd(elem, "frame"), body, body->frames[i]);
-    }
-
-    // otherwise skip
-    else {
-      Body(elem, body, body->frames[i]);
-    }
-  }
-
   // write child bodies recursively
   for (int i=0; i<body->bodies.size(); i++) {
-    if (body->bodies[i]->frame != frame) continue;
-    Body(InsertEnd(elem, "body"), body->bodies[i], nullptr);
+    XMLElement* celem = OneFrame(elem, body->bodies[i]->frame);
+    Body(InsertEnd(celem, "body"), body->bodies[i]);
   }
 }
 

src/xml/xml_native_writer.h

@@ -49,7 +49,7 @@ class mjXWriter : public mjXBase {
   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 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
@@ -73,6 +73,7 @@ class mjXWriter : public mjXBase {
   void OneTendon(tinyxml2::XMLElement* elem,    mjCTendon* ptendon,     mjCDef* def);
   void OneActuator(tinyxml2::XMLElement* elem,  mjCActuator* pactuator, mjCDef* def);
   void OnePlugin(tinyxml2::XMLElement* elem,    mjsPlugin* plugin);
+  tinyxml2::XMLElement* OneFrame(tinyxml2::XMLElement* elem, mjCFrame* frame);
 
   bool writingdefaults;                       // true during defaults write
 };

src/xml/xml_util.cc

@@ -344,7 +344,8 @@ bool mjXSchema::NameMatch(XMLElement* elem, int level) {
   if (name_ == "body" &&
       ((level == 1 && !strcmp(elem->Value(), "worldbody")) ||
       (level != 1 && !strcmp(elem->Value(), "body")) ||
-      (level >= 1 && !strcmp(elem->Value(), "frame")))) {
+      (level >= 1 && !strcmp(elem->Value(), "frame")) ||
+      (level >= 1 && !strcmp(elem->Value(), "replicate")))) {
     return true;
   }