Add mjs_getParent to retrieve the parent body of an object.

PiperOrigin-RevId: 715794859
Change-Id: Ib9792787cdb966f579938dde3f94ebabca8ef1fc

src/user/user_api.cc

@@ -649,6 +649,30 @@ mjsBody* mjs_findChild(mjsBody* bodyspec, const char* name) {
 
 
 
+// get parent body
+mjsBody* mjs_getParent(mjsElement* element) {
+  switch (element->elemtype) {
+    case mjOBJ_BODY:
+      return &(static_cast<mjCBody*>(element)->GetParent()->spec);
+    case mjOBJ_FRAME:
+      return &(static_cast<mjCFrame*>(element)->GetParent()->spec);
+    case mjOBJ_JOINT:
+      return &(static_cast<mjCJoint*>(element)->GetParent()->spec);
+    case mjOBJ_GEOM:
+      return &(static_cast<mjCGeom*>(element)->GetParent()->spec);
+    case mjOBJ_SITE:
+      return &(static_cast<mjCSite*>(element)->GetParent()->spec);
+    case mjOBJ_CAMERA:
+      return &(static_cast<mjCCamera*>(element)->GetParent()->spec);
+    case mjOBJ_LIGHT:
+      return &(static_cast<mjCLight*>(element)->GetParent()->spec);
+    default:
+      return nullptr;
+  }
+}
+
+
+
 // find frame by name
 mjsFrame* mjs_findFrame(mjSpec* s, const char* name) {
   mjsElement* frame = mjs_findElement(s, mjOBJ_FRAME, name);

src/user/user_api.h

@@ -201,6 +201,9 @@ MJAPI mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name);
 // Find child body by name.
 MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name);
 
+// Get parent body.
+MJAPI mjsBody* mjs_getParent(mjsElement* element);
+
 // Find frame by name.
 MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name);
 

src/user/user_objects.h

@@ -347,6 +347,7 @@ class mjCBody : public mjCBody_, private mjsBody {
 
   // set parent of this body
   void SetParent(mjCBody* _body) { parent = _body; }
+  mjCBody* GetParent() const { return parent; }
 
  private:
   mjCBody(const mjCBody& other, mjCModel* _model);  // copy constructor
@@ -411,6 +412,7 @@ class mjCFrame : public mjCFrame_, private mjsFrame {
   void CopyFromSpec(void);
   void PointToLocal(void);
   void SetParent(mjCBody* _body) { body = _body; }
+  mjCBody* GetParent() const { return body; }
 
   mjCFrame& operator+=(const mjCBody& other);
 
@@ -462,6 +464,7 @@ class mjCJoint : public mjCJoint_, private mjsJoint {
 
   void CopyFromSpec(void);
   void SetParent(mjCBody* _body) { body = _body; }
+  mjCBody* GetParent() const { return body; }
 
   // used by mjXWriter and mjCModel
   const std::vector<double>& get_userdata() const { return userdata_; }
@@ -543,6 +546,7 @@ class mjCGeom : public mjCGeom_, private mjsGeom {
   bool IsVisual(void) const { return visual_; }
   void SetNotVisual(void) { visual_ = false; }
   void SetParent(mjCBody* _body) { body = _body; }
+  mjCBody* GetParent() const { return body; }
   mjtGeom Type() const { return type; }
 
   // Compute all coefs modeling the interaction with the surrounding fluid.
@@ -608,6 +612,7 @@ class mjCSite : public mjCSite_, private mjsSite {
   // site's body
   mjCBody* Body() const { return body; }
   void SetParent(mjCBody* _body) { body = _body; }
+  mjCBody* GetParent() const { return body; }
 
   // use strings from mjCBase rather than mjStrings from mjsSite
   using mjCBase::name;
@@ -661,6 +666,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera {
   const std::vector<double>& get_userdata() const { return userdata_; }
 
   void SetParent(mjCBody* _body) { body = _body; }
+  mjCBody* GetParent() const { return body; }
 
  private:
   void Compile(void);                     // compiler
@@ -701,6 +707,7 @@ class mjCLight : public mjCLight_, private mjsLight {
   const std::string& get_targetbody() const { return targetbody_; }
 
   void SetParent(mjCBody* _body) { body = _body; }
+  mjCBody* GetParent() const { return body; }
 
  private:
   void Compile(void);                     // compiler