Add mjmBodyPair to C API.

PiperOrigin-RevId: 606997065
Change-Id: Iefc2dff6ae942ce93ca83ec5f5fe9d7ae8bb6a40

src/user/user_api.cc

@@ -185,6 +185,15 @@ mjmPair* mjm_addPair(void* model, void* defspec) {
 
 
 
+// add pair exclusion to model
+mjmExclude* mjm_addExclude(void* model) {
+  mjCModel* modelC = static_cast<mjCModel*>(model);
+  mjCBodyPair* bodypair = modelC->AddExclude();
+  return &bodypair->spec;
+}
+
+
+
 // add equality to model
 mjmEquality* mjm_addEquality(void* model, void* defspec) {
   mjCModel* modelC = static_cast<mjCModel*>(model);

src/user/user_api.h

@@ -428,6 +428,15 @@ typedef struct _mjmPair {
 } mjmPair;
 
 
+typedef struct _mjmExclude {
+  mjElement element;               // internal, do not modify
+  mjString name;                   // name
+  mjString bodyname1;              // name of geom 1
+  mjString bodyname2;              // name of geom 2
+  mjString info;                   // message appended to errors
+} mjmExclude;
+
+
 typedef struct _mjmEquality {      // equality specification
   mjElement element;               // internal, do not modify
   mjString name;                   // name
@@ -648,6 +657,9 @@ MJAPI mjmMaterial* mjm_addMaterial(void* model, void* defspec);
 // Add pair to model.
 MJAPI mjmPair* mjm_addPair(void* model, void* defspec);
 
+// Add excluded body pair to model.
+MJAPI mjmExclude* mjm_addExclude(void *model);
+
 // Add equality to model.
 MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec);
 

src/user/user_composite.cc

@@ -864,9 +864,9 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
 
   // exclude contact pair
   if (!last) {
-    mjCBodyPair* exclude = model->AddExclude();
-    exclude->bodyname1 = this_body;
-    exclude->bodyname2 = next_body;
+    mjmExclude* exclude = mjm_addExclude(model);
+    mjm_setString(exclude->bodyname1, std::string(this_body).c_str());
+    mjm_setString(exclude->bodyname2, std::string(next_body).c_str());
   }
 
   // add site at the boundary
@@ -939,9 +939,9 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err
     mju_copy(eq->solimp, solimpsmooth, mjNIMP);
 
     // remove contact between connected bodies
-    mjCBodyPair* pair = model->AddExclude();
-    pair->bodyname1 = txt;
-    pair->bodyname2 = txt2;
+    mjmExclude* pair = mjm_addExclude(model);
+    mjm_setString(pair->bodyname1, std::string(txt).c_str());
+    mjm_setString(pair->bodyname2, std::string(txt2).c_str());
   }
 
   return true;

src/user/user_objects.cc

@@ -3585,34 +3585,59 @@ mjCBodyPair::mjCBodyPair(mjCModel* _model) {
   model = _model;
 
   // set defaults
-  bodyname1.clear();
-  bodyname2.clear();
+  spec_bodyname1_.clear();
+  spec_bodyname2_.clear();
 
   // clear internal variables
   body1 = body2 = signature = -1;
+
+  PointToLocal();
+  CopyFromSpec();
+}
+
+
+
+void mjCBodyPair::PointToLocal() {
+  spec.element = (mjElement)this;
+  spec.name = (mjString)&name;
+  spec.bodyname1 = (mjString)&spec_bodyname1_;
+  spec.bodyname2 = (mjString)&spec_bodyname2_;
+  spec.info = (mjString)&info;
+}
+
+
+
+void mjCBodyPair::CopyFromSpec() {
+  *static_cast<mjmExclude*>(this) = spec;
+  bodyname1_ = spec_bodyname1_;
+  bodyname2_ = spec_bodyname2_;
+  bodyname1 = (mjString)&bodyname1_;
+  bodyname2 = (mjString)&bodyname2_;
 }
 
 
 
 // compiler
 void mjCBodyPair::Compile(void) {
+  CopyFromSpec();
+
   // find body 1
-  mjCBody* pb1 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname1);
+  mjCBody* pb1 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname1_);
   if (!pb1) {
-    throw mjCError(this, "body '%s' not found in bodypair %d", bodyname1.c_str(), id);
+    throw mjCError(this, "body '%s' not found in bodypair %d", bodyname1_.c_str(), id);
   }
 
   // find body 2
-  mjCBody* pb2 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname2);
+  mjCBody* pb2 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname2_);
   if (!pb2) {
-    throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2.c_str(), id);
+    throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2_.c_str(), id);
   }
 
   // swap if body1 > body2
   if (pb1->id > pb2->id) {
-    string nametmp = bodyname1;
-    bodyname1 = bodyname2;
-    bodyname2 = nametmp;
+    string nametmp = bodyname1_;
+    bodyname1_ = bodyname2_;
+    bodyname2_ = nametmp;
 
     mjCBody* bodytmp = pb1;
     pb1 = pb2;

src/user/user_objects.h

@@ -984,26 +984,37 @@ class mjCPair : public mjCBase, private mjmPair {
 //------------------------- class mjCBodyPair ------------------------------------------------------
 // Body pair specification, use to exclude pairs
 
-class mjCBodyPair : public mjCBase {
+class mjCBodyPair : public mjCBase, private mjmExclude {
   friend class mjCBody;
   friend class mjCModel;
 
  public:
-  // parameters set by user
-  std::string bodyname1;          // name of geom 1
-  std::string bodyname2;          // name of geom 2
+  mjmExclude spec;
+  using mjCBase::name;
+  using mjCBase::info;
+
+  void CopyFromSpec();
+  void PointToLocal();
+
+  std::string get_bodyname1() const { return bodyname1_; }
+  std::string get_bodyname2() const { return bodyname2_; }
 
   int GetSignature(void) {
     return signature;
   }
 
  private:
-  mjCBodyPair(mjCModel*);             // constructor
-  void Compile(void);                 // compiler
+  mjCBodyPair(mjCModel*);          // constructor
+  void Compile(void);              // compiler
 
-  int body1;                      // id of body1
-  int body2;                      // id of body2
-  int signature;                  // body1<<16 + body2
+  int body1;                       // id of body1
+  int body2;                       // id of body2
+  int signature;                   // body1<<16 + body2
+
+  std::string bodyname1_;          // name of geom 1
+  std::string bodyname2_;          // name of geom 2
+  std::string spec_bodyname1_;
+  std::string spec_bodyname2_;
 };
 
 

src/xml/xml_native_reader.cc

@@ -3424,13 +3424,20 @@ void mjXReader::Contact(XMLElement* section) {
 
     // body pair to exclude
     else if (name=="exclude") {
-      mjCBodyPair* pexclude = model->AddExclude();
-      GetXMLPos(elem, pexclude);
+      mjmExclude* pexclude = mjm_addExclude(model);
+      string exname, exbody1, exbody2;
+
+      // write error info
+      mjm_setString(pexclude->info, ("line = " + std::to_string(elem->GetLineNum())).c_str());
 
       // read name and body names
-      ReadAttrTxt(elem, "name", pexclude->name);
-      ReadAttrTxt(elem, "body1", pexclude->bodyname1, true);
-      ReadAttrTxt(elem, "body2", pexclude->bodyname2, true);
+      if (ReadAttrTxt(elem, "name", exname)) {
+        mjm_setString(pexclude->name, exname.c_str());
+      }
+      ReadAttrTxt(elem, "body1", exbody1, true);
+      mjm_setString(pexclude->bodyname1, exbody1.c_str());
+      ReadAttrTxt(elem, "body2", exbody2, true);
+      mjm_setString(pexclude->bodyname2, exbody2.c_str());
     }
 
     // advance to next element

src/xml/xml_native_writer.cc

@@ -1543,8 +1543,8 @@ void mjXWriter::Contact(XMLElement* root) {
 
     // write attributes
     WriteAttrTxt(elem, "name", pexclude->name);
-    WriteAttrTxt(elem, "body1", pexclude->bodyname1);
-    WriteAttrTxt(elem, "body2", pexclude->bodyname2);
+    WriteAttrTxt(elem, "body1", pexclude->get_bodyname1());
+    WriteAttrTxt(elem, "body2", pexclude->get_bodyname2());
   }
 }