Add mjm_attachBody to the C API.

PiperOrigin-RevId: 617207000
Change-Id: I1fe59ba0d79938a4a383b115997c31dd6c71b8e5

src/user/user_objects.cc

@@ -683,6 +683,7 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
     for (int i=0; i<other.frames.size(); i++) {
       frames.push_back(new mjCFrame(*other.frames[i]));
       frames.back()->model = model;
+      frames.back()->body = this;
       fmap[other.frames[i]] = i;
     }
 
@@ -731,6 +732,13 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
 
 
 
+mjCBody& mjCBody::operator+=(mjCBody& other) {
+  bodies.push_back(&other);
+  return *this;
+}
+
+
+
 void mjCBody::PointToLocal() {
   spec.element = (mjElement)this;
   spec.name = (mjString)&name;
@@ -784,6 +792,21 @@ mjCBody::~mjCBody() {
 
 
 
+// apply prefix and suffix, propagate to children
+void mjCBody::SetNameSpace() {
+  if (!name.empty()) {
+    name = prefix + name + suffix;
+  }
+
+  for (auto& body : bodies) {
+    body->prefix = prefix;
+    body->suffix = suffix;
+    body->SetNameSpace();
+  }
+}
+
+
+
 // create child body and add it to body
 mjCBody* mjCBody::AddBody(mjCDef* _def) {
   // create body
@@ -1317,6 +1340,7 @@ mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) {
   mjm_defaultFrame(spec);
   compiled = false;
   model = _model;
+  body = NULL;
   frame = _frame ? _frame : NULL;
   PointToLocal();
   CopyFromSpec();
@@ -1341,6 +1365,27 @@ mjCFrame& mjCFrame::operator=(const mjCFrame& other) {
 }
 
 
+// attach body to frame
+mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
+  mjCBody* subtree = new mjCBody(other, model);
+  subtree->SetFrame(this);
+  subtree->SetNameSpace();
+
+  // add to tree
+  *body += *subtree;
+
+  // TODO: needs to attach only referencing elements
+  *model += *other.model;
+  return *this;
+}
+
+
+
+void mjCFrame::SetParent(mjCBody* _body) {
+  body = _body;
+}
+
+
 
 void mjCFrame::PointToLocal() {
   spec.element = (mjElement)this;