From 13d6eeacdd258d17a9d671a055f257003133e48b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 12 Feb 2024 09:24:39 -0800 Subject: [PATCH] Add mjmPair to C API. PiperOrigin-RevId: 606274243 Change-Id: I15e2a38096a61bc1377a666c96113ae20ce3ed59 --- src/user/user_api.cc | 12 ++++++- src/user/user_api.h | 25 +++++++++++++++ src/user/user_init.cc | 14 +++++++++ src/user/user_objects.cc | 61 ++++++++++++++++++++++++------------ src/user/user_objects.h | 28 ++++++++++------- src/xml/xml_native_reader.cc | 29 +++++++++++------ src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 18 +++++------ 8 files changed, 137 insertions(+), 52 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index f29a7429..2aeea3d6 100644 --- a/src/user/user_api.cc +++ b/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(model); mjCDef* def = static_cast(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(model); + mjCDef* def = static_cast(defspec); + mjCPair* pair = modelC->AddPair(def); + return &pair->spec; +} + + + // add equality to model mjmEquality* mjm_addEquality(void* model, void* defspec) { mjCModel* modelC = static_cast(model); diff --git a/src/user/user_api.h b/src/user/user_api.h index d904f0d4..c2327f5a 100644 --- a/src/user/user_api.h +++ b/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 distpair.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(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; - geomname1 = geomname2; - geomname2 = nametmp; + string nametmp = geomname1_; + geomname1_ = geomname2_; + geomname2_ = nametmp; mjCGeom* geomtmp = geom1; geom1 = geom2; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5265842e..47f4398d 100644 --- a/src/user/user_objects.h +++ b/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 - std::string geomname1; // name of geom 1 - std::string geomname2; // name of geom 2 + mjmPair spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; - // 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 distclassname); - ReadAttrTxt(elem, "geom1", ppair->geomname1, true); - ReadAttrTxt(elem, "geom2", ppair->geomname2, true); + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(ppair->classname, classname.c_str()); + } + 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); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index c3b85317..459d0a06 100644 --- a/src/xml/xml_native_reader.h +++ b/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); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 03f56732..5fe4965f 100644 --- a/src/xml/xml_native_writer.cc +++ b/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, "geom2", ppair->geomname2); + WriteAttrTxt(elem, "geom1", ppair->get_geomname1()); + WriteAttrTxt(elem, "geom2", ppair->get_geomname2()); } // defaults and regular WriteAttrTxt(elem, "name", ppair->name); - WriteAttrInt(elem, "condim", ppair->condim, def->pair.condim); - WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin); - WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap); - WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref); - WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction); - WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp); - WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction); + WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim); + WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin); + WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap); + WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref); + WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction); + WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp); + WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction); }