Add mjmPair to C API.

PiperOrigin-RevId: 606274243
Change-Id: I15e2a38096a61bc1377a666c96113ae20ce3ed59

src/user/user_api.cc

@@ -137,7 +137,7 @@ mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
 
 
 
-// Add material to model.
+// add material to model
 mjmMaterial* mjm_addMaterial(void* model, void* defspec) {
   mjCModel* modelC = static_cast<mjCModel*>(model);
   mjCDef* def = static_cast<mjCDef*>(defspec);
@@ -147,6 +147,16 @@ mjmMaterial* mjm_addMaterial(void* model, void* defspec) {
 
 
 
+// add pair to model
+mjmPair* mjm_addPair(void* model, void* defspec) {
+  mjCModel* modelC = static_cast<mjCModel*>(model);
+  mjCDef* def = static_cast<mjCDef*>(defspec);
+  mjCPair* pair = modelC->AddPair(def);
+  return &pair->spec;
+}
+
+
+
 // add equality to model
 mjmEquality* mjm_addEquality(void* model, void* defspec) {
   mjCModel* modelC = static_cast<mjCModel*>(model);

src/user/user_api.h

@@ -325,6 +325,25 @@ typedef struct _mjmMaterial {      // material specification
 } mjmMaterial;
 
 
+typedef struct _mjmPair {
+  mjElement element;               // internal, do not modify
+  mjString name;                   // name
+  mjString classname;              // class name
+  mjString geomname1;              // name of geom 1
+  mjString geomname2;              // name of geom 2
+
+  // optional parameters: computed from geoms if not set by user
+  int condim;                      // contact dimensionality
+  mjtNum solref[mjNREF];           // solver reference, normal direction
+  mjtNum solreffriction[mjNREF];   // solver reference, frictional directions
+  mjtNum solimp[mjNIMP];           // solver impedance
+  double margin;                   // margin for contact detection
+  double gap;                      // include in solver if dist<margin-gap
+  double friction[5];              // full contact friction
+  mjString info;                   // message appended to errors
+} mjmPair;
+
+
 typedef struct _mjmEquality {      // equality specification
   mjElement element;               // internal, do not modify
   mjString name;                   // name
@@ -492,6 +511,9 @@ MJAPI mjmFlex* mjm_addFlex(void* model);
 // Add material to model.
 MJAPI mjmMaterial* mjm_addMaterial(void* model, void* defspec);
 
+// Add pair to model.
+MJAPI mjmPair* mjm_addPair(void* model, void* defspec);
+
 // Add equality to model.
 MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec);
 
@@ -597,6 +619,9 @@ MJAPI void mjm_defaultFlex(mjmFlex& flex);
 // Default material attributes.
 MJAPI void mjm_defaultMaterial(mjmMaterial& material);
 
+// Default pair attributes.
+MJAPI void mjm_defaultPair(mjmPair& pair);
+
 // Default equality attributes.
 MJAPI void mjm_defaultEquality(mjmEquality& equality);
 

src/user/user_init.cc

@@ -205,6 +205,20 @@ void mjm_defaultMaterial(mjmMaterial& material) {
 
 
 
+// default pair attributes
+void mjm_defaultPair(mjmPair& pair) {
+  memset(&pair, 0, sizeof(mjmPair));
+  pair.condim = 3;
+  mj_defaultSolRefImp(pair.solref, pair.solimp);
+  pair.friction[0] = 1;
+  pair.friction[1] = 1;
+  pair.friction[2] = 0.005;
+  pair.friction[3] = 0.0001;
+  pair.friction[4] = 0.0001;
+}
+
+
+
 // default equality attributes
 void mjm_defaultEquality(mjmEquality& equality) {
   memset(&equality, 0, sizeof(mjmEquality));

src/user/user_objects.cc

@@ -3341,20 +3341,11 @@ void mjCMaterial::Compile(void) {
 
 // constructor
 mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
-  // set defaults
+  mjm_defaultPair(spec);
-  geomname1.clear();
-  geomname2.clear();
 
-  condim = 3;
+  // set defaults
-  mj_defaultSolRefImp(solref, solimp);
+  spec_geomname1_.clear();
-  mju_zero(solreffriction, mjNREF);
+  spec_geomname2_.clear();
-  margin = 0;
-  gap = 0;
-  friction[0] = 1;
-  friction[1] = 1;
-  friction[2] = 0.005;
-  friction[3] = 0.0001;
-  friction[4] = 0.0001;
 
   // clear internal variables
   geom1 = nullptr;
@@ -3363,33 +3354,63 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
 
   // reset to default if given
   if (_def) {
+    _def->pair.CopyFromSpec();
     *this = _def->pair;
   }
 
   // set model, def
   model = _model;
   def = (_def ? _def : (_model ? _model->defaults[0] : 0));
+
+  // point to local (needs to be after defaults)
+  PointToLocal();
+
+  // in case this camera is not compiled
+  CopyFromSpec();
+}
+
+
+
+void mjCPair::PointToLocal() {
+  spec.element = (mjElement)this;
+  spec.name = (mjString)&name;
+  spec.classname = (mjString)&classname;
+  spec.geomname1 = (mjString)&spec_geomname1_;
+  spec.geomname2 = (mjString)&spec_geomname2_;
+  spec.info = (mjString)&info;
+}
+
+
+
+void mjCPair::CopyFromSpec() {
+  *static_cast<mjmPair*>(this) = spec;
+  geomname1_ = spec_geomname1_;
+  geomname2_ = spec_geomname2_;
+  geomname1 = (mjString)&geomname1_;
+  geomname2 = (mjString)&geomname2_;
 }
 
 
 
 // compiler
 void mjCPair::Compile(void) {
+  CopyFromSpec();
+
   // check condim
   if (condim!=1 && condim!=3 && condim!=4 && condim!=6) {
     throw mjCError(this, "invalid condim in collision %d", "", id);
   }
 
   // find geom 1
-  geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1);
+  geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1_);
   if (!geom1) {
-    throw mjCError(this, "geom '%s' not found in collision %d", geomname1.c_str(), id);
+    throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id);
   }
 
   // find geom 2
-  geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2);
+  geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2_);
   if (!geom2) {
-    throw mjCError(this, "geom '%s' not found in collision %d", geomname2.c_str(), id);
+    throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id);
   }
 
   // mark geoms as not visual
@@ -3398,9 +3419,9 @@ void mjCPair::Compile(void) {
 
   // swap if body1 > body2
   if (geom1->body->id > geom2->body->id) {
-    string nametmp = geomname1;
+    string nametmp = geomname1_;
-    geomname1 = geomname2;
+    geomname1_ = geomname2_;
-    geomname2 = nametmp;
+    geomname2_ = nametmp;
 
     mjCGeom* geomtmp = geom1;
     geom1 = geom2;

src/user/user_objects.h

@@ -966,24 +966,23 @@ class mjCMaterial : public mjCBase, private mjmMaterial {
 //------------------------- class mjCPair ----------------------------------------------------------
 // Predefined geom pair for collision detection
 
-class mjCPair : public mjCBase {
+class mjCPair : public mjCBase, private mjmPair {
   friend class mjCDef;
   friend class mjCBody;
   friend class mjCModel;
+  friend class mjXWriter;
 
  public:
-  // parameters set by user
+  mjmPair spec;
-  std::string geomname1;          // name of geom 1
+  using mjCBase::name;
-  std::string geomname2;          // name of geom 2
+  using mjCBase::classname;
+  using mjCBase::info;
 
-  // optional parameters: computed from geoms if not set by user
+  void CopyFromSpec();
-  int condim;                     // contact dimensionality
+  void PointToLocal();
-  mjtNum solref[mjNREF];          // solver reference, normal direction
+
-  mjtNum solreffriction[mjNREF];  // solver reference, frictional directions
+  std::string get_geomname1() { return geomname1_; }
-  mjtNum solimp[mjNIMP];          // solver impedance
+  std::string get_geomname2() { return geomname2_; }
-  double margin;                  // margin for contact detection
-  double gap;                     // include in solver if dist<margin-gap
-  double friction[5];             // full contact friction
 
   int GetSignature(void) {
     return signature;
@@ -996,6 +995,11 @@ class mjCPair : public mjCBase {
   mjCGeom* geom1;                 // geom1
   mjCGeom* geom2;                 // geom2
   int signature;                  // body1<<16 + body2
+
+  std::string geomname1_;
+  std::string geomname2_;
+  std::string spec_geomname1_;
+  std::string spec_geomname2_;
 };
 
 

src/xml/xml_native_reader.cc

@@ -1751,18 +1751,26 @@ void mjXReader::OneLight(XMLElement* elem, mjmLight* plight) {
 
 
 // pair element parser
-void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) {
+void mjXReader::OnePair(XMLElement* elem, mjmPair* ppair) {
-  string text;
+  string text, name, classname, geomname1, geomname2;
 
   // regular only
   if (!readingdefaults) {
-    ReadAttrTxt(elem, "class", ppair->classname);
+    if (ReadAttrTxt(elem, "class", classname)) {
-    ReadAttrTxt(elem, "geom1", ppair->geomname1, true);
+      mjm_setString(ppair->classname, classname.c_str());
-    ReadAttrTxt(elem, "geom2", ppair->geomname2, true);
+    }
+    if (ReadAttrTxt(elem, "geom1", geomname1)) {
+      mjm_setString(ppair->geomname1, geomname1.c_str());
+    }
+    if (ReadAttrTxt(elem, "geom2", geomname2)) {
+      mjm_setString(ppair->geomname2, geomname2.c_str());
+    }
   }
 
   // read other parameters
-  ReadAttrTxt(elem, "name", ppair->name);
+  if (ReadAttrTxt(elem, "name", name)) {
+    mjm_setString(ppair->name, name.c_str());
+  }
   ReadAttrInt(elem, "condim", &ppair->condim);
   ReadAttr(elem, "solref", mjNREF, ppair->solref, text, false, false);
   ReadAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, text, false, false);
@@ -1771,7 +1779,9 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) {
   ReadAttr(elem, "gap", 1, &ppair->gap, text);
   ReadAttr(elem, "friction", 5, ppair->friction, text, false, false);
 
-  GetXMLPos(elem, ppair);
+  // write error info
+  mjm_setString(ppair->info,
+      std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
 }
 
 
@@ -2626,7 +2636,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
     else if (name=="light") OneLight(elem, &def->light.spec);
 
     // read pair
-    else if (name=="pair") OnePair(elem, &def->pair);
+    else if (name=="pair") OnePair(elem, &def->pair.spec);
 
     // read equality
     else if (name=="equality") OneEquality(elem, &def->equality.spec);
@@ -2658,6 +2668,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
     mjm_finalize(def->equality.spec.element);
     mjm_finalize(def->tendon.spec.element);
     mjm_finalize(def->flex.spec.element);
+    mjm_finalize(def->pair.spec.element);
 
     // advance
     elem = NextSiblingElement(elem);
@@ -3338,7 +3349,7 @@ void mjXReader::Contact(XMLElement* section) {
     // geom pair to include
     if (name=="pair") {
       // create pair and parse
-      mjCPair* ppair = model->AddPair(def);
+      mjmPair* ppair = mjm_addPair(model, def);
       OnePair(elem, ppair);
     }
 

src/xml/xml_native_reader.h

@@ -62,7 +62,7 @@ class mjXReader : public mjXBase {
   void OneSite(tinyxml2::XMLElement* elem, mjmSite& site);
   void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera);
   void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight);
-  void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair);
+  void OnePair(tinyxml2::XMLElement* elem, mjmPair* ppair);
   void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality);
   void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon);
   void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator);

src/xml/xml_native_writer.cc

@@ -523,19 +523,19 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
   // regular
   if (!writingdefaults) {
     WriteAttrTxt(elem, "class", ppair->classname);
-    WriteAttrTxt(elem, "geom1", ppair->geomname1);
+    WriteAttrTxt(elem, "geom1", ppair->get_geomname1());
-    WriteAttrTxt(elem, "geom2", ppair->geomname2);
+    WriteAttrTxt(elem, "geom2", ppair->get_geomname2());
   }
 
   // defaults and regular
   WriteAttrTxt(elem, "name", ppair->name);
-  WriteAttrInt(elem, "condim", ppair->condim, def->pair.condim);
+  WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim);
-  WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin);
+  WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin);
-  WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap);
+  WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap);
-  WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref);
+  WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref);
-  WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction);
+  WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction);
-  WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp);
+  WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp);
-  WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction);
+  WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction);
 }