Associate children of deleted body to the newly created frame in mjs_bodyToFrame.

Also associate all children that need a parent body to the parent of the deleted body.

This is a bug that was causing the former children of the body that gets transformed to a frame to not be children of the new frame.

PiperOrigin-RevId: 702702853
Change-Id: I36d48f0446bc87c665b18d34e0b0609d82c70f51

src/user/user_objects.h

@@ -344,6 +344,9 @@ class mjCBody : public mjCBody_, private mjsBody {
 
   mjsFrame* last_attached;  // last attached frame to this body
 
+  // set parent of this body
+  void SetParent(const mjCBody* _body) { parentid = _body->id; }
+
  private:
   mjCBody(const mjCBody& other, mjCModel* _model);  // copy constructor
   mjCBody& operator=(const mjCBody& other);         // copy assignment
@@ -406,7 +409,7 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
 
   void CopyFromSpec(void);
   void PointToLocal(void);
-  void SetParent(mjCBody* _body);
+  void SetParent(mjCBody* _body) { body = _body; }
 
   mjCFrame& operator+=(const mjCBody& other);
 
@@ -457,6 +460,7 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
   using mjCBase::info;
 
   void CopyFromSpec(void);
+  void SetParent(mjCBody* _body) { body = _body; }
 
   // used by mjXWriter and mjCModel
   const std::vector<double>& get_userdata() const { return userdata_; }
@@ -537,6 +541,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
   void SetInertia(void);              // compute and set geom inertia
   bool IsVisual(void) const { return visual_; }
   void SetNotVisual(void) { visual_ = false; }
+  void SetParent(mjCBody* _body) { body = _body; }
   mjtGeom Type() const { return type; }
 
   // Compute all coefs modeling the interaction with the surrounding fluid.
@@ -601,6 +606,7 @@ class mjCSite : public mjCSite_, private mjsSite {
 
   // site's body
   mjCBody* Body() const { return body; }
+  void SetParent(mjCBody* _body) { body = _body; }
 
   // use strings from mjCBase rather than mjStrings from mjsSite
   using mjCBase::name;
@@ -653,6 +659,8 @@ class mjCCamera : public mjCCamera_, private mjsCamera {
   const std::string& get_targetbody() const { return targetbody_; }
   const std::vector<double>& get_userdata() const { return userdata_; }
 
+  void SetParent(mjCBody* _body) { body = _body; }
+
  private:
   void Compile(void);                     // compiler
   void CopyFromSpec(void);
@@ -691,6 +699,8 @@ class mjCLight : public mjCLight_, private mjsLight {
   // used by mjXWriter and mjCModel
   const std::string& get_targetbody() const { return targetbody_; }
 
+  void SetParent(mjCBody* _body) { body = _body; }
+
  private:
   void Compile(void);                     // compiler
   void CopyFromSpec(void);