Add error checking in mjs_setFrame.

Also raise error if the frame is not found in the mjCBody copy constructor.

Fixes #2543.

PiperOrigin-RevId: 742624013
Change-Id: I4999b3165c97f8d079412214d027ca7a8dae8cb3

src/user/user_api.cc

@@ -758,13 +758,19 @@ mjsFrame* mjs_findFrame(mjSpec* s, const char* name) {
 
 
 // set frame
-void mjs_setFrame(mjsElement* dest, mjsFrame* frame) {
+int mjs_setFrame(mjsElement* dest, mjsFrame* frame) {
   if (!frame) {
-    return;
+    return -1;
   }
   mjCFrame* frameC = static_cast<mjCFrame*>(frame->element);
   mjCBase* baseC = static_cast<mjCBase*>(dest);
-  baseC->SetFrame(frameC);
+  try {
+    baseC->SetFrame(frameC);
+    return 0;
+  } catch (mjCError& e) {
+    baseC->model->SetError(e);
+    return -1;
+  }
 }
 
 

src/user/user_api.h

@@ -370,8 +370,8 @@ MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size);
 // Set element's default.
 MJAPI void mjs_setDefault(mjsElement* element, const mjsDefault* def);
 
-// Set element's enlcosing frame.
-MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame);
+// Set element's enclosing frame, return 0 on success.
+MJAPI int mjs_setFrame(mjsElement* dest, mjsFrame* frame);
 
 // Resolve alternative orientations to quat, return error if any.
 MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,

src/user/user_objects.cc

@@ -773,6 +773,9 @@ void mjCBase::SetFrame(mjCFrame* _frame) {
   if (!_frame) {
     return;
   }
+  if (_frame->body && GetParent() != _frame->body) {
+    throw mjCError(this, "Frame and body '%s' have mismatched parents", name.c_str());
+  }
   frame = _frame;
 }
 
@@ -891,8 +894,18 @@ mjCBody& mjCBody::operator+=(const mjCBody& other) {
   for (int i=0; i < other.bodies.size(); i++) {
     bodies.push_back(new mjCBody(*other.bodies[i], model));  // triggers recursive call
     bodies.back()->parent = this;
-    bodies.back()->frame =
-      other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : nullptr;
+    bodies.back()->frame = nullptr;
+    if (other.bodies[i]->frame) {
+      if (fmap.find(other.bodies[i]->frame) != fmap.end()) {
+        bodies.back()->frame = frames[fmap[other.bodies[i]->frame]];
+      } else {
+        throw mjCError(this, "Frame '%s' not found in other body",
+                       other.bodies[i]->frame->name.c_str());
+      }
+      if (bodies.back()->frame && bodies.back()->frame->body != this) {
+        throw mjCError(this, "Frame and body '%s' have mismatched parents", name.c_str());
+      }
+    }
   }
 
   return *this;

src/user/user_objects.h

@@ -254,6 +254,9 @@ class mjCBase : public mjCBase_ {
   // Copy plugins instantiated in this object
   virtual void CopyPlugin() {}
 
+  // Returns parent of this object
+  virtual mjCBase* GetParent() const { return nullptr; }
+
   // Copy assignment
   mjCBase& operator=(const mjCBase& other);
 

src/xml/xml_native_reader.cc

@@ -3647,7 +3647,9 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
         }
       } else {
         // only set frame to existing body
-        mjs_setFrame(child->element, pframe);
+        if (mjs_setFrame(child->element, pframe)) {
+          throw mjXError(elem, mjs_getError(spec));
+        }
       }
     }