Add mjs_attachFrame to user_api.

PiperOrigin-RevId: 625648309
Change-Id: I4d6dfc4d38ad6ce688df83eb7cc2c34676076193

src/user/user_api.cc

@@ -91,6 +91,17 @@ int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
 
 
 
+// attach frame to a parent body
+int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
+                    const char* prefix, const char* suffix) {
+  mjCBody* body_parent = static_cast<mjCBody*>(parent->element);
+  mjCFrame* child_frame = static_cast<mjCFrame*>(child->element);
+  *body_parent += std::string(prefix) + *child_frame + std::string(suffix);
+  return 0;
+}
+
+
+
 // get error message from model
 const char* mjs_getError(mjSpec* s) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);

src/user/user_api.h

@@ -750,6 +750,10 @@ MJAPI mjModel* mjs_compile(mjSpec* s, const mjVFS* vfs);
 MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
                          const char* prefix, const char* suffix);
 
+// Attach child frame to a parent body, return 0 if success
+MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
+                          const char* prefix, const char* suffix);
+
 // Get error message from model.
 MJAPI const char* mjs_getError(mjSpec* s);
 

src/user/user_objects.cc

@@ -729,6 +729,49 @@ mjCBody& mjCBody::operator+=(const mjCBody& other) {
 
 
 
+// attach frame to body
+mjCBody& mjCBody::operator+=(const mjCFrame& other) {
+  mjCBody* subtree = other.body;
+  other.model->prefix = other.prefix;
+  other.model->suffix = other.suffix;
+
+  // map old frames to indices
+  std::map<mjCFrame*, int> fmap;
+  for (int i=0; i<subtree->frames.size(); i++) {
+    fmap[subtree->frames[i]] = i;
+  }
+
+  // copy children that are inside the input frame
+  CopyList(frames, subtree->frames, fmap, &other);  // needs to be done first
+  CopyList(geoms, subtree->geoms, fmap, &other);
+  CopyList(joints, subtree->joints, fmap, &other);
+  CopyList(sites, subtree->sites, fmap, &other);
+  CopyList(cameras, subtree->cameras, fmap, &other);
+  CopyList(lights, subtree->lights, fmap, &other);
+
+  for (int i=0; i<subtree->bodies.size(); i++) {
+    if (subtree->bodies[i]->frame != &other) {
+      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;
+  }
+
+  // name space
+  this->NameSpace(other.model);
+
+  // attach referencing elements
+  *model += *other.model;
+
+  // clear namespace and return body
+  other.model->prefix.clear();
+  other.model->suffix.clear();
+  return *this;
+}
+
+
+
 // copy src list of elements into dst; set body, model and frame
 template <typename T>
 void mjCBody::CopyList(std::vector<T*>& dst, const std::vector<T*>& src,

src/user/user_objects.h

@@ -275,6 +275,7 @@ class mjCBody : public mjCBody_, private mjsBody {
 
   // API for adding existing objects to body
   mjCBody& operator+=(const mjCBody& other);
+  mjCBody& operator+=(const mjCFrame& other);
 
   // API for accessing objects
   int NumObjects(mjtObj type);
@@ -360,7 +361,6 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
   void SetParent(mjCBody* _body);
 
   mjCFrame& operator+=(const mjCBody& other);
-  mjCFrame& operator+=(const mjCFrame& other);
 
  private:
   mjCFrame(mjCModel* = 0, mjCFrame* = 0);      // constructor