Add mjs_attachToSite for attaching a body to a site.

PiperOrigin-RevId: 684432183
Change-Id: I7100bebfb6521f9dbfbb07f31490b2b9298bf18c

src/user/user_api.cc

@@ -29,6 +29,7 @@
 #include "user/user_model.h"
 #include "user/user_objects.h"
 #include "user/user_cache.h"
+#include "user/user_util.h"
 
 namespace {
 
@@ -160,6 +161,29 @@ mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
 
 
 
+// attach child body to a parent site
+mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child,
+                           const char* prefix, const char* suffix) {
+  if (!parent) {
+    mju_error("parent site is null");
+    return nullptr;
+  }
+  mjCSite* site = static_cast<mjCSite*>(parent->element);
+  mjCBody* body = site->Body();
+  mjCFrame* frame = body->AddFrame(site->frame);
+  frame->spec.pos[0] = site->spec.pos[0];
+  frame->spec.pos[1] = site->spec.pos[1];
+  frame->spec.pos[2] = site->spec.pos[2];
+  frame->spec.quat[0] = site->spec.quat[0];
+  frame->spec.quat[1] = site->spec.quat[1];
+  frame->spec.quat[2] = site->spec.quat[2];
+  frame->spec.quat[3] = site->spec.quat[3];
+  frame->SetParent(body);
+  return mjs_attachBody(&frame->spec, child, prefix, suffix);
+}
+
+
+
 // get error message from model
 const char* mjs_getError(mjSpec* s) {
   mjCModel* modelC = static_cast<mjCModel*>(s->element);
@@ -521,15 +545,8 @@ mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* p
 
 
 // get spec from body
-mjSpec* mjs_getSpec(mjsBody* body) {
-  return &(static_cast<mjCBody*>(body->element)->model->spec);
-}
-
-
-
-// get spec from frame
-mjSpec* mjs_getSpecFromFrame(mjsFrame* frame) {
-  return &(static_cast<mjCFrame*>(frame->element)->model->spec);
+mjSpec* mjs_getSpec(mjsElement* element) {
+  return &(static_cast<mjCBase*>(element)->model->spec);
 }
 
 

src/user/user_api.h

@@ -71,11 +71,15 @@ MJAPI int mjs_activatePlugin(mjSpec* s, const char* name);
 
 // 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);
+                              const char* prefix, const char* suffix);
 
 // 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);
+                                const char* prefix, const char* suffix);
+
+// Attach child body to a parent site, return the attached body if success or NULL otherwise.
+MJAPI mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child,
+                                const char* prefix, const char* suffix);
 
 // Detach body from mjSpec, remove all references and delete the body, return 0 on success.
 MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b);
@@ -186,10 +190,7 @@ MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def);
 //---------------------------------- Find/get utilities --------------------------------------------
 
 // Get spec from body.
-MJAPI mjSpec* mjs_getSpec(mjsBody* body);
-
-// Get spec from frame.
-MJAPI mjSpec* mjs_getSpecFromFrame(mjsFrame* frame);
+MJAPI mjSpec* mjs_getSpec(mjsElement* element);
 
 // Find spec (model asset) by name.
 MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name);

src/user/user_objects.h

@@ -587,6 +587,9 @@ class mjCSite : public mjCSite_, private mjsSite {
 
   mjsSite spec;                   // variables set by user
 
+  // site's body
+  mjCBody* Body() const { return body; }
+
   // use strings from mjCBase rather than mjStrings from mjsSite
   using mjCBase::name;
   using mjCBase::info;