From 80a986c0f548b83f8d3f7ed1eaebe86595b105be Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 14 Feb 2024 08:39:01 -0800 Subject: [PATCH] Add mjmBodyPair to C API. PiperOrigin-RevId: 606997065 Change-Id: Iefc2dff6ae942ce93ca83ec5f5fe9d7ae8bb6a40 --- src/user/user_api.cc | 9 ++++++++ src/user/user_api.h | 12 ++++++++++ src/user/user_composite.cc | 12 +++++----- src/user/user_objects.cc | 43 ++++++++++++++++++++++++++++-------- src/user/user_objects.h | 29 ++++++++++++++++-------- src/xml/xml_native_reader.cc | 17 +++++++++----- src/xml/xml_native_writer.cc | 4 ++-- 7 files changed, 95 insertions(+), 31 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 0853531f..1b146cfa 100644 --- a/src/user/user_api.cc +++ b/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(model); + mjCBodyPair* bodypair = modelC->AddExclude(); + return &bodypair->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 00562314..29a55fa6 100644 --- a/src/user/user_api.h +++ b/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); diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index c008a7b8..ff2edb62 100644 --- a/src/user/user_composite.cc +++ b/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; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4ca06f14..a7b30490 100644 --- a/src/user/user_objects.cc +++ b/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(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; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5205371e..5cb9a3a9 100644 --- a/src/user/user_objects.h +++ b/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_; }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c4a79a6d..f7eb37c0 100644 --- a/src/xml/xml_native_reader.cc +++ b/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 diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 6bfaf22a..16a0a59a 100644 --- a/src/xml/xml_native_writer.cc +++ b/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()); } }