Return attached body/frame in mjs_attachBody/Frame.

PiperOrigin-RevId: 679601525
Change-Id: Icdc4616808e14bf0a3ebc17d89392382d084c31b

src/user/user_api.cc

@@ -117,33 +117,37 @@ mjModel* mj_compile(mjSpec* s, const mjVFS* vfs) {
 
 
 // attach body to a frame of the parent
-int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
-                   const char* prefix, const char* suffix) {
+mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child,
+                        const char* prefix, const char* suffix) {
   mjCFrame* frame_parent = static_cast<mjCFrame*>(parent->element);
   mjCBody* child_body = static_cast<mjCBody*>(child->element);
   try {
     *frame_parent += std::string(prefix) + *child_body + std::string(suffix);
   } catch (mjCError& e) {
     frame_parent->model->SetError(e);
-    return -1;
+    return nullptr;
   }
-  return 0;
+  mjsBody* attached_body = frame_parent->last_attached;
+  frame_parent->last_attached = nullptr;
+  return attached_body;
 }
 
 
 
 // attach frame to a parent body
-int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
-                    const char* prefix, const char* suffix) {
+mjsFrame* 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);
   try {
     *body_parent += std::string(prefix) + *child_frame + std::string(suffix);
   } catch (mjCError& e) {
     body_parent->model->SetError(e);
-    return -1;
+    return nullptr;
   }
-  return 0;
+  mjsFrame* attached_frame = body_parent->last_attached;
+  body_parent->last_attached = nullptr;
+  return attached_frame;
 }
 
 

src/user/user_api.h

@@ -66,12 +66,12 @@ MJAPI void mjs_addSpec(mjSpec* s, mjSpec* child);
 
 //---------------------------------- Attachment ----------------------------------------------------
 
-// Attach child body to a parent frame, return 0 on success.
-MJAPI int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
+// Attach child body to a parent frame, return the attached body if success or NULL otherwise.
+MJAPI mjsBody* mjs_attachBody(mjsFrame* parent, const mjsBody* child,
                          const char* prefix, const char* suffix);
 
-// Attach child frame to a parent body, return 0 on success.
-MJAPI int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
+// Attach child frame to a parent body, return the attached frame if success or NULL otherwise.
+MJAPI mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
                           const char* prefix, const char* suffix);
 
 // Detach body from mjSpec, remove all references and delete the body, return 0 on success.

src/user/user_objects.cc

@@ -762,6 +762,7 @@ mjCBody::mjCBody(mjCModel* _model) {
   margin = 0;
   mjuu_zerovec(xpos0, 3);
   mjuu_setvec(xquat0, 1, 0, 0, 0);
+  last_attached = nullptr;
 
   // clear object lists
   bodies.clear();
@@ -860,7 +861,9 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) {
   frames.back()->body = this;
   frames.back()->model = model;
   frames.back()->frame = other.frame;
+  frames.back()->NameSpace(other.model);
   int i = frames.size();
+  last_attached = &frames.back()->spec;
 
   // map input frames to index in this->frames
   std::map<mjCFrame*, int> fmap;
@@ -1723,6 +1726,7 @@ mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) {
   model = _model;
   body = NULL;
   frame = _frame ? _frame : NULL;
+  last_attached = nullptr;
   PointToLocal();
   CopyFromSpec();
 }
@@ -1771,6 +1775,7 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
 
   // add to body children
   body->bodies.push_back(subtree);
+  last_attached = &body->bodies.back()->spec;
 
   // attach referencing elements
   *model += *other.model;

src/user/user_objects.h

@@ -331,6 +331,8 @@ class mjCBody : public mjCBody_, private mjsBody {
   mjtNum* mpos(const std::string& state_name);
   mjtNum* mquat(const std::string& state_name);
 
+  mjsFrame* last_attached;  // last attached frame to this body
+
  private:
   mjCBody(const mjCBody& other, mjCModel* _model);  // copy constructor
   mjCBody& operator=(const mjCBody& other);         // copy assignment
@@ -398,6 +400,8 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
 
   bool IsAncestor(const mjCFrame* child) const;  // true if child is contained in this frame
 
+  mjsBody* last_attached;  // last attached body to this frame
+
  private:
   void Compile(void);                          // compiler
 

src/xml/xml_native_reader.cc

@@ -3627,7 +3627,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
         UpdateString(suffix, count, i);
 
         // attach to parent
-        if (mjs_attachFrame(body, pframe, /*prefix=*/"", suffix.c_str()) != 0) {
+        if (!mjs_attachFrame(body, pframe, /*prefix=*/"", suffix.c_str())) {
           throw mjXError(elem, mjs_getError(spec));
         }
       }
@@ -3704,7 +3704,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
         if (!child) {
           throw mjXError(0, "could not find body '%s''%s'", body_name.c_str());
         }
-        if (mjs_attachBody(pframe, child, prefix.c_str(), "") != 0) {
+        if (!mjs_attachBody(pframe, child, prefix.c_str(), "")) {
           throw mjXError(elem, mjs_getError(spec));
         }
       } else {