From 1bd6a32af269feb777956d7c11e7031766d65ff1 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sat, 10 Feb 2024 03:29:22 -0800 Subject: [PATCH] Add mjmEquality, mjmTendon and mjmWrap to C API. PiperOrigin-RevId: 605853716 Change-Id: Ieb468cdec5a3032200cf1121960408a61bb08fc5 --- src/user/user_api.cc | 56 +++++++++++++++ src/user/user_api.h | 71 ++++++++++++++++++++ src/user/user_composite.cc | 78 ++++++++++----------- src/user/user_flexcomp.cc | 6 +- src/user/user_init.cc | 33 +++++++++ src/user/user_model.cc | 8 +-- src/user/user_objects.cc | 125 +++++++++++++++++++++++----------- src/user/user_objects.h | 68 +++++++++++-------- src/xml/xml_native_reader.cc | 127 +++++++++++++++++++++-------------- src/xml/xml_native_reader.h | 4 +- src/xml/xml_native_writer.cc | 22 +++--- 11 files changed, 423 insertions(+), 175 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 3dfc5db5..3c12ed5f 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -123,6 +123,62 @@ void* mjm_addFrame(mjmBody* bodyspec, void* parentframe) { +// add equality to model +mjmEquality* mjm_addEquality(void* model, void* defspec) { + mjCModel* modelC = static_cast(model); + mjCDef* def = static_cast(defspec); + mjCEquality* equality = modelC->AddEquality(def); + return &equality->spec; +} + + + +// add tendon to model +mjmTendon* mjm_addTendon(void* model, void* defspec) { + mjCModel* modelC = static_cast(model); + mjCDef* def = static_cast(defspec); + mjCTendon* tendon = modelC->AddTendon(def); + return &tendon->spec; +} + + + +// wrap site using tendon +MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendonspec, const char* name) { + mjCTendon* tendon = reinterpret_cast(tendonspec->element); + tendon->WrapSite(name); + return &tendon->path.back()->spec; +} + + + +// wrap geom using tendon +mjmWrap* mjm_wrapGeom(mjmTendon* tendonspec, const char* name, const char* sidesite) { + mjCTendon* tendon = reinterpret_cast(tendonspec->element); + tendon->WrapGeom(name, sidesite); + return &tendon->path.back()->spec; +} + + + +// wrap joint using tendon +mjmWrap* mjm_wrapJoint(mjmTendon* tendonspec, const char* name, double coef) { + mjCTendon* tendon = reinterpret_cast(tendonspec->element); + tendon->WrapJoint(name, coef); + return &tendon->path.back()->spec; +} + + + +// wrap pulley using tendon +mjmWrap* mjm_wrapPulley(mjmTendon* tendonspec, double divisor) { + mjCTendon* tendon = reinterpret_cast(tendonspec->element); + tendon->WrapPulley(divisor); + return &tendon->path.back()->spec; +} + + + // add actuator to model mjmActuator* mjm_addActuator(void* model, void* defspec) { mjCModel* modelC = static_cast(model); diff --git a/src/user/user_api.h b/src/user/user_api.h index d56d9191..faa6cd93 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -211,6 +211,53 @@ typedef struct _mjmLight { } mjmLight; +typedef struct _mjmEquality { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + mjtEq type; // constraint type + mjString name1; // name of object 1 + mjString name2; // name of object 2 + mjtByte active; // initial activation state + mjtNum solref[mjNREF]; // solver reference + mjtNum solimp[mjNIMP]; // solver impedance + double data[mjNEQDATA]; // type-dependent data + mjString info; // message appended to errors +} mjmEquality; + + +typedef struct _mjmTendon { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + int group; // group for visualization + int limited; // does tendon have limits: 0 false, 1 true, 2 auto + double width; // width for rendering + mjtNum solref_limit[mjNREF]; // solver reference: tendon limits + mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits + mjtNum solref_friction[mjNREF]; // solver reference: tendon friction + mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction + double range[2]; // length limits + double margin; // margin value for tendon limit detection + double stiffness; // stiffness coefficient + double damping; // damping coefficient + double frictionloss; // friction loss + double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring + mjString material; // name of material for rendering + mjDouble userdata; // user data + float rgba[4]; // rgba when material is omitted + mjString info; // message appended to errors +} mjmTendon; + + +typedef struct _mjmWrap { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + mjString info; // message appended to errors +} mjmWrap; + + typedef struct _mjmActuator { mjElement element; // compiler only, do not modify mjString name; // name @@ -299,6 +346,24 @@ MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec); // Add frame to body. MJAPI void* mjm_addFrame(mjmBody* body, void* parentframe); +// Add equality to model. +MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec); + +// Add tendon to model. +MJAPI mjmTendon* mjm_addTendon(void* model, void* defspec); + +// Wrap site using tendon. +MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendon, const char* name); + +// Wrap geom using tendon. +MJAPI mjmWrap* mjm_wrapGeom(mjmTendon* tendon, const char* name, const char* sidesite); + +// Wrap joint using tendon. +MJAPI mjmWrap* mjm_wrapJoint(mjmTendon* tendon, const char* name, double coef); + +// Wrap pulley using tendon. +MJAPI mjmWrap* mjm_wrapPulley(mjmTendon* tendon, double divisor); + // Add actuator to model. MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec); @@ -365,6 +430,12 @@ MJAPI void mjm_defaultCamera(mjmCamera& camera); // Default light attributes. MJAPI void mjm_defaultLight(mjmLight& light); +// Default equality attributes. +MJAPI void mjm_defaultEquality(mjmEquality& equality); + +// Default tendon attributes. +MJAPI void mjm_defaultTendon(mjmTendon& tendon); + // Default actuator attributes. MJAPI void mjm_defaultActuator(mjmActuator& actuator); diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 715cb7d2..7ab810d3 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -139,7 +139,7 @@ void mjCComposite::SetDefault(void) { for (int i=0; iAddTendon(def + mjCOMPKIND_TENDON); - ten->def = model->defaults[0]; - ten->name = txt0; + mjmTendon* ten = mjm_addTendon(model, def + mjCOMPKIND_TENDON); + mjm_setDefault(ten->element, model->defaults[0]); + mjm_setString(ten->name, txt0); ten->group = 4; - ten->WrapSite(txt1); - ten->WrapSite(txt2); + mjm_wrapSite(ten, txt1); + mjm_wrapSite(ten, txt2); // add equality constraint - mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TENDON); - eq->def = model->defaults[0]; + mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_TENDON); + mjm_setDefault(eq->element, model->defaults[0]); eq->type = mjEQ_TENDON; - eq->name1 = ten->name; + mjm_setString(eq->name1, mjm_getString(ten->name)); } } @@ -657,10 +657,10 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err ten->WrapSite(txt2); // add equality constraint - mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TENDON); - eq->def = model->defaults[0]; + mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_TENDON); + mjm_setDefault(eq->element, model->defaults[0]); eq->type = mjEQ_TENDON; - eq->name1 = ten->name; + mjm_setString(eq->name1, ten->name.c_str()); } } } @@ -928,12 +928,12 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err char txt2[200]; // add equality constraint - mjCEquality* eq = model->AddEquality(); + mjmEquality* eq = mjm_addEquality(model, 0); eq->type = mjEQ_CONNECT; mju::sprintf_arr(txt, "%sB0", prefix.c_str()); mju::sprintf_arr(txt2, "%sB%d", prefix.c_str(), count[0]-1); - eq->name1 = txt; - eq->name2 = txt2; + mjm_setString(eq->name1, txt); + mjm_setString(eq->name2, txt2); mjuu_setvec(eq->data, -0.5*spacing, 0, 0); mju_copy(eq->solref, solrefsmooth, mjNREF); mju_copy(eq->solimp, solimpsmooth, mjNIMP); @@ -1019,10 +1019,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i mjuu_setvec(jnt->axis, 1, 0, 0); // add constraint - mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TWIST); - eq->def = model->defaults[0]; + mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_TWIST); + mjm_setDefault(eq->element, model->defaults[0]); eq->type = mjEQ_JOINT; - eq->name1 = mjm_getString(jnt->name); + mjm_setString(eq->name1, mjm_getString(jnt->name)); } // add stretch joint @@ -1037,10 +1037,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i mjuu_setvec(jnt->axis, 1, 0, 0); // add constraint - mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_STRETCH); - eq->def = model->defaults[0]; + mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_STRETCH); + mjm_setDefault(eq->element, model->defaults[0]); eq->type = mjEQ_JOINT; - eq->name1 = mjm_getString(jnt->name); + mjm_setString(eq->name1, mjm_getString(jnt->name)); } return body; @@ -1158,10 +1158,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro mjuu_setvec(jnt->axis, 0, 0, 1); // add fix constraint - mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_JOINT); - eq->def = model->defaults[0]; + mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_JOINT); + mjm_setDefault(eq->element, model->defaults[0]); eq->type = mjEQ_JOINT; - eq->name1 = mjm_getString(jnt->name); + mjm_setString(eq->name1, mjm_getString(jnt->name)); // add joint to tendon ten->WrapJoint(std::string(mjm_getString(jnt->name)), 1); @@ -1178,12 +1178,12 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro char txt2[200]; mju::sprintf_arr(txt2, "%sJ%d_%d_%d", prefix.c_str(), ix1, iy1, iz1); - mjCEquality* eqn = model->AddEquality(); + mjmEquality* eqn = mjm_addEquality(model, 0); mju_copy(eqn->solref, solrefsmooth, mjNREF); mju_copy(eqn->solimp, solimpsmooth, mjNIMP); eqn->type = mjEQ_JOINT; - eqn->name1 = txt; - eqn->name2 = txt2; + mjm_setString(eqn->name1, txt); + mjm_setString(eqn->name2, txt2); } } } @@ -1192,10 +1192,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro } // finalize fixed tendon - mjCEquality* eqt = model->AddEquality(def + mjCOMPKIND_TENDON); - eqt->def = model->defaults[0]; + mjmEquality* eqt = mjm_addEquality(model, def + mjCOMPKIND_TENDON); + mjm_setDefault(eqt->element, model->defaults[0]); eqt->type = mjEQ_TENDON; - eqt->name1 = ten->name; + mjm_setString(eqt->name1, ten->name.c_str()); // skin if (skin) { @@ -1228,10 +1228,10 @@ void mjCComposite::MakeShear(mjCModel* model) { ten->name = txt; // equality constraint - mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_SHEAR); - eq->def = model->defaults[0]; + mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_SHEAR); + mjm_setDefault(eq->element, model->defaults[0]); eq->type = mjEQ_TENDON; - eq->name1 = txt; + mjm_setString(eq->name1, txt); } } } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index a817d7b2..072a1698 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -478,11 +478,11 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz // create edge equality constraint if (equality) { - mjCEquality *pe = model->AddEquality(&def); - pe->def = model->defaults[0]; + mjmEquality* pe = mjm_addEquality(model, &def); + mjm_setDefault(pe->element, model->defaults[0]); pe->type = mjEQ_FLEX; pe->active = true; - pe->name1 = name; + mjm_setString(pe->name1, name.c_str()); } return true; diff --git a/src/user/user_init.cc b/src/user/user_init.cc index e2af855f..805a17f2 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -134,6 +134,39 @@ void mjm_defaultLight(mjmLight& light) { +// Default equality attributes. +void mjm_defaultEquality(mjmEquality& equality) { + memset(&equality, 0, sizeof(mjmEquality)); + equality.type = mjEQ_CONNECT; + equality.active = 1; + mj_defaultSolRefImp(equality.solref, equality.solimp); + equality.data[1] = 1; + equality.data[10] = 1; // torque:force ratio +} + + + +// default tendon attributes +void mjm_defaultTendon(mjmTendon& tendon) { + memset(&tendon, 0, sizeof(mjmTendon)); + tendon.group = 0; + tendon.width = 0.003; + tendon.limited = 2; + tendon.range[0] = 0; + tendon.range[1] = 0; + mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit); + mj_defaultSolRefImp(tendon.solref_friction, tendon.solimp_friction); + tendon.margin = 0; + tendon.stiffness = 0; + tendon.damping = 0; + tendon.frictionloss = 0; + tendon.springlength[0] = tendon.springlength[1] = -1; + tendon.rgba[0] = tendon.rgba[1] = tendon.rgba[2] = 0.5f; + tendon.rgba[3] = 1.0f; +} + + + // default actuator attributes void mjm_defaultActuator(mjmActuator& actuator) { memset(&actuator, 0, sizeof(mjmActuator)); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index b7c1f0f9..9e24fa3d 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2022,7 +2022,7 @@ void mjCModel::CopyObjects(mjModel* m) { // find equality constraint referencing this flex m->flex_edgeequality[i] = 0; for (int k=0; k<(int)equalities.size(); k++) { - if (equalities[k]->type==mjEQ_FLEX && equalities[k]->name1==pfl->name) { + if (equalities[k]->type==mjEQ_FLEX && equalities[k]->name1_==pfl->name) { m->flex_edgeequality[i] = 1; break; } @@ -2251,7 +2251,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1]; - copyvec(m->tendon_user+nuser_tendon*i, pte->userdata.data(), nuser_tendon); + copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); copyvec(m->tendon_rgba+4*i, pte->rgba, 4); // set wraps @@ -2914,7 +2914,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { if (nuser_tendon == -1) { nuser_tendon = 0; for (int i=0; iuserdata.size()); + nuser_tendon = mjMAX(nuser_tendon, tendons[i]->spec_userdata_.size()); } } if (nuser_actuator == -1) { @@ -3480,7 +3480,7 @@ bool mjCModel::CopyBack(const mjModel* m) { tendons[i]->frictionloss = (double)m->tendon_frictionloss[i]; if (nuser_tendon) { - copyvec(tendons[i]->userdata.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); + copyvec(tendons[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3fb88a34..50eac241 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -474,7 +474,7 @@ void mjCDef::Compile(const mjCModel* model) { geom.userdata_.resize(model->nuser_geom); site.userdata_.resize(model->nuser_site); camera.userdata_.resize(model->nuser_cam); - tendon.userdata.resize(model->nuser_tendon); + tendon.userdata_.resize(model->nuser_tendon); actuator.userdata_.resize(model->nuser_actuator); } @@ -3509,33 +3509,55 @@ void mjCBodyPair::Compile(void) { // initialize default constraint mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) { - // set defaults - type = mjEQ_CONNECT; - name1.clear(); - name2.clear(); - active = true; - mj_defaultSolRefImp(solref, solimp); - - mjuu_zerovec(data, mjNEQDATA); - data[1] = 1; - data[10] = 1; // torque:force ratio + mjm_defaultEquality(spec); // clear internal variables + spec_name1_.clear(); + spec_name2_.clear(); obj1id = obj2id = -1; // reset to default if given if (_def) { + _def->equality.CopyFromSpec(); *this = _def->equality; } // 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 mjCEquality::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.name1 = (mjString)&spec_name1_; + spec.name2 = (mjString)&spec_name2_; + spec.info = (mjString)&info; +} + + +void mjCEquality::CopyFromSpec() { + *static_cast(this) = spec; + name1_ = spec_name1_; + name2_ = spec_name2_; + name1 = (mjString)&name1_; + name2 = (mjString)&name2_; } // compiler void mjCEquality::Compile(void) { + CopyFromSpec(); + mjtObj objtype; mjCBase *px1, *px2; mjtJoint jt1, jt2; @@ -3554,17 +3576,17 @@ void mjCEquality::Compile(void) { } // find object 1, get id - px1 = model->FindObject(objtype, name1); + px1 = model->FindObject(objtype, name1_); if (!px1) { - throw mjCError(this, "unknown element '%s' in equality constraint %d", name1.c_str(), id); + throw mjCError(this, "unknown element '%s' in equality constraint %d", name1_.c_str(), id); } obj1id = px1->id; // find object 2, get id - if (!name2.empty()) { - px2 = model->FindObject(objtype, name2); + if (!name2_.empty()) { + px2 = model->FindObject(objtype, name2_); if (!px2) { - throw mjCError(this, "unknown element '%s' in equality constraint %d", name2.c_str(), id); + throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id); } obj2id = px2->id; } @@ -3587,12 +3609,12 @@ void mjCEquality::Compile(void) { // make sure flex is not rigid if (type==mjEQ_FLEX && model->flexes[obj1id]->rigid) { - throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1.c_str(), id); + throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id); } // make sure the two objects are different if (obj1id==obj2id) { - throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1.c_str(), id); + throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1_.c_str(), id); } // make sure joints are scalar @@ -3613,25 +3635,11 @@ void mjCEquality::Compile(void) { // constructor mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { - // tendon defaults - group = 0; - material_.clear(); - width = 0.003; - limited = 2; - range[0] = 0; - range[1] = 0; - mj_defaultSolRefImp(solref_limit, solimp_limit); - mj_defaultSolRefImp(solref_friction, solimp_friction); - margin = 0; - stiffness = 0; - damping = 0; - frictionloss = 0; - springlength[0] = springlength[1] = -1; - rgba[0] = rgba[1] = rgba[2] = 0.5f; - rgba[3] = 1.0f; - userdata.clear(); + mjm_defaultTendon(spec); // clear internal variables + spec_material_.clear(); + spec_userdata_.clear(); path.clear(); matid = -1; @@ -3643,6 +3651,33 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { // 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 mjCTendon::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.material = (mjString)&spec_material_; + spec.userdata = (mjDouble)&spec_userdata_; + spec.info = (mjString)&info; +} + + + +void mjCTendon::CopyFromSpec() { + *static_cast(this) = spec; + material_ = spec_material_; + userdata_ = spec_userdata_; + material = (mjString)&material_; + userdata = (mjDouble)&userdata_; } @@ -3741,12 +3776,14 @@ mjCWrap* mjCTendon::GetWrap(int id) { // compiler void mjCTendon::Compile(void) { + CopyFromSpec(); + // resize userdata - if (userdata.size() > model->nuser_tendon) { + if (userdata_.size() > model->nuser_tendon) { throw mjCError(this, "user has more values than nuser_tendon in tendon '%s' (id = %d)", name.c_str(), id); } - userdata.resize(model->nuser_tendon); + userdata_.resize(model->nuser_tendon); // check for empty path int sz = (int)path.size(); @@ -3880,6 +3917,18 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) { sideid = -1; prm = 0; sidesite.clear(); + + // point to local + PointToLocal(); +} + + + +void mjCWrap::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.info = (mjString)&info; } @@ -4528,7 +4577,7 @@ void mjCSensor::Compile(void) { } // make sure tendon has limit - if (!((mjCTendon*)obj)->limited) { + if (!((mjCTendon*)obj)->spec.limited) { throw mjCError(this, "tendon must be limited in sensor '%s' (id = %d)", name.c_str(), id); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 3cc25fbc..d37c388b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1016,21 +1016,20 @@ class mjCBodyPair : public mjCBase { //------------------------- class mjCEquality ------------------------------------------------------ // Describes an equality constraint -class mjCEquality : public mjCBase { +class mjCEquality : public mjCBase, private mjmEquality { friend class mjCDef; friend class mjCBody; friend class mjCModel; friend class mjXWriter; public: - // variables set by user - mjtEq type; // constraint type - std::string name1; // name of object 1 - std::string name2; // name of object 2 - bool active; // initial activation state - mjtNum solref[mjNREF]; // solver reference - mjtNum solimp[mjNIMP]; // solver impedance - double data[mjNEQDATA]; // type-dependent data + mjmEquality spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + + void CopyFromSpec(); + void PointToLocal(); private: mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor @@ -1038,6 +1037,11 @@ class mjCEquality : public mjCBase { int obj1id; // id of object 1 int obj2id; // id of object 2 + + std::string name1_; + std::string name2_; + std::string spec_name1_; + std::string spec_name2_; }; @@ -1045,12 +1049,17 @@ class mjCEquality : public mjCBase { //------------------------- class mjCTendon -------------------------------------------------------- // Describes a tendon -class mjCTendon : public mjCBase { +class mjCTendon : public mjCBase, private mjmTendon { friend class mjCDef; friend class mjCModel; friend class mjXWriter; public: + mjmTendon spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + void set_material(std::string _material) { material_ = _material; } std::string& get_material() { return material_; } void del_material() { material_.clear(); } @@ -1064,32 +1073,26 @@ class mjCTendon : public mjCBase { // API for access to wrapping objects int NumWraps(void); // number of wraps mjCWrap* GetWrap(int); // pointer to wrap + std::vector path; // wrapping objects - // variables set by user - int group; // group for visualization - int limited; // does tendon have limits: 0 false, 1 true, 2 auto - double width; // width for rendering - mjtNum solref_limit[mjNREF]; // solver reference: tendon limits - mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits - mjtNum solref_friction[mjNREF]; // solver reference: tendon friction - mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction - double range[2]; // length limits - double margin; // margin value for tendon limit detection - double stiffness; // stiffness coefficient - double damping; // damping coefficient - double frictionloss; // friction loss - double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring - std::vector userdata; // user data - float rgba[4]; // rgba when material is omitted + // used by mjXWriter and mjCModel + const std::vector& get_userdata() { return userdata_; } + + void CopyFromSpec(); + void PointToLocal(); private: mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor ~mjCTendon(); // destructor void Compile(void); // compiler - std::string material_; // name of material for rendering int matid; // material id for rendering - std::vector path; // wrapping objects + + // variable-size data + std::string material_; + std::string spec_material_; + std::vector userdata_; + std::vector spec_userdata_; }; @@ -1097,11 +1100,18 @@ class mjCTendon : public mjCBase { //------------------------- class mjCWrap ---------------------------------------------------------- // Describes a tendon wrap object -class mjCWrap : public mjCBase { +class mjCWrap : public mjCBase, private mjmWrap { friend class mjCTendon; friend class mjCModel; public: + mjmWrap spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + + void PointToLocal(); + mjtWrap type; // wrap object type mjCBase* obj; // wrap object pointer int sideid; // side site id; -1 if not applicable diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 4387868a..3c81f253 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1751,9 +1751,9 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) { // equality element parser -void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { +void mjXReader::OneEquality(XMLElement* elem, mjmEquality* pequality) { int n; - string text; + string text, name1, name2, name, classname; // read type (bad keywords already detected by schema) text = elem->Value(); @@ -1761,19 +1761,23 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { // regular only if (!readingdefaults) { - ReadAttrTxt(elem, "name", pequality->name); - ReadAttrTxt(elem, "class", pequality->classname); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pequality->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(pequality->classname, classname.c_str()); + }; switch (pequality->type) { case mjEQ_CONNECT: - ReadAttrTxt(elem, "body1", pequality->name1, true); - ReadAttrTxt(elem, "body2", pequality->name2); + ReadAttrTxt(elem, "body1", name1, true); + ReadAttrTxt(elem, "body2", name2); ReadAttr(elem, "anchor", 3, pequality->data, text, true); break; case mjEQ_WELD: - ReadAttrTxt(elem, "body1", pequality->name1, true); - ReadAttrTxt(elem, "body2", pequality->name2); + ReadAttrTxt(elem, "body1", name1, true); + ReadAttrTxt(elem, "body2", name2); ReadAttr(elem, "relpose", 7, pequality->data+3, text); ReadAttr(elem, "torquescale", 1, pequality->data+10, text); if (!ReadAttr(elem, "anchor", 3, pequality->data, text)) { @@ -1782,19 +1786,19 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { break; case mjEQ_JOINT: - ReadAttrTxt(elem, "joint1", pequality->name1, true); - ReadAttrTxt(elem, "joint2", pequality->name2); + ReadAttrTxt(elem, "joint1", name1, true); + ReadAttrTxt(elem, "joint2", name2); ReadAttr(elem, "polycoef", 5, pequality->data, text); break; case mjEQ_TENDON: - ReadAttrTxt(elem, "tendon1", pequality->name1, true); - ReadAttrTxt(elem, "tendon2", pequality->name2); + ReadAttrTxt(elem, "tendon1", name1, true); + ReadAttrTxt(elem, "tendon2", name2); ReadAttr(elem, "polycoef", 5, pequality->data, text); break; case mjEQ_FLEX: - ReadAttrTxt(elem, "flex", pequality->name1, true); + ReadAttrTxt(elem, "flex", name1, true); break; case mjEQ_DISTANCE: @@ -1804,6 +1808,11 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { default: // SHOULD NOT OCCUR throw mjXError(elem, "unrecognized equality constraint type"); } + + mjm_setString(pequality->name1, name1.c_str()); + if (!name2.empty()) { + mjm_setString(pequality->name2, name2.c_str()); + } } // read attributes @@ -1813,20 +1822,29 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { ReadAttr(elem, "solref", mjNREF, pequality->solref, text, false, false); ReadAttr(elem, "solimp", mjNIMP, pequality->solimp, text, false, false); - GetXMLPos(elem, pequality); + // write error info + mjm_setString(pequality->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } // tendon element parser -void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) { - string text; +void mjXReader::OneTendon(XMLElement* elem, mjmTendon* pten) { + string text, name, classname, material; + std::vector userdata; // read attributes - ReadAttrTxt(elem, "name", pten->name); - ReadAttrTxt(elem, "class", pten->classname); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pten->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(pten->classname, classname.c_str()); + } ReadAttrInt(elem, "group", &pten->group); - ReadAttrTxt(elem, "material", pten->get_material()); + if (ReadAttrTxt(elem, "material", material)) { + mjm_setString(pten->material, material.c_str()); + } MapValue(elem, "limited", &pten->limited, TFAuto_map, 3); ReadAttr(elem, "width", 1, &pten->width, text); ReadAttr(elem, "solreflimit", mjNREF, pten->solref_limit, text, false, false); @@ -1845,9 +1863,13 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) { ReadAttr(elem, "rgba", 4, pten->rgba, text); // read userdata - ReadVector(elem, "user", pten->userdata, text); + if (ReadVector(elem, "user", userdata, text)) { + mjm_setDouble(pten->userdata, userdata.data(), userdata.size()); + } - GetXMLPos(elem, pten); + // write error info + mjm_setString(pten->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2280,8 +2302,8 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) { ReadAttr(ejnt, "axis", 3, el->joint.spec.axis, text); // solreffix, solimpfix - ReadAttr(ejnt, "solreffix", mjNREF, el->equality.solref, text, false, false); - ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.solimp, text, false, false); + ReadAttr(ejnt, "solreffix", mjNREF, el->equality.spec.solref, text, false, false); + ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.spec.solimp, text, false, false); // joint attributes MapValue(elem, "limited", &el->joint.spec.limited, TFAuto_map, 3); @@ -2312,26 +2334,28 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) { comp.add[kind] = true; // solreffix, solimpfix - ReadAttr(eten, "solreffix", mjNREF, comp.def[kind].equality.solref, text, false, false); - ReadAttr(eten, "solimpfix", mjNIMP, comp.def[kind].equality.solimp, text, false, false); + ReadAttr(eten, "solreffix", mjNREF, comp.def[kind].equality.spec.solref, text, false, false); + ReadAttr(eten, "solimpfix", mjNIMP, comp.def[kind].equality.spec.solimp, text, false, false); // tendon attributes - MapValue(elem, "limited", &comp.def[kind].tendon.limited, TFAuto_map, 3); - ReadAttrInt(eten, "group", &comp.def[kind].tendon.group); - ReadAttr(eten, "solreflimit", mjNREF, comp.def[kind].tendon.solref_limit, text, false, false); - ReadAttr(eten, "solimplimit", mjNIMP, comp.def[kind].tendon.solimp_limit, text, false, false); + std::string material; + MapValue(elem, "limited", &comp.def[kind].tendon.spec.limited, TFAuto_map, 3); + ReadAttrInt(eten, "group", &comp.def[kind].tendon.spec.group); + ReadAttr(eten, "solreflimit", mjNREF, comp.def[kind].tendon.spec.solref_limit, text, false, false); + ReadAttr(eten, "solimplimit", mjNIMP, comp.def[kind].tendon.spec.solimp_limit, text, false, false); ReadAttr(eten, - "solreffriction", mjNREF, comp.def[kind].tendon.solref_friction, text, false, false); + "solreffriction", mjNREF, comp.def[kind].tendon.spec.solref_friction, text, false, false); ReadAttr(eten, - "solimpfriction", mjNIMP, comp.def[kind].tendon.solimp_friction, text, false, false); - ReadAttr(eten, "range", 2, comp.def[kind].tendon.range, text); - ReadAttr(eten, "margin", 1, &comp.def[kind].tendon.margin, text); - ReadAttr(eten, "stiffness", 1, &comp.def[kind].tendon.stiffness, text); - ReadAttr(eten, "damping", 1, &comp.def[kind].tendon.damping, text); - ReadAttr(eten, "frictionloss", 1, &comp.def[kind].tendon.frictionloss, text); - ReadAttrTxt(eten, "material", comp.def[kind].tendon.get_material()); - ReadAttr(eten, "rgba", 4, comp.def[kind].tendon.rgba, text); - ReadAttr(eten, "width", 1, &comp.def[kind].tendon.width, text); + "solimpfriction", mjNIMP, comp.def[kind].tendon.spec.solimp_friction, text, false, false); + ReadAttr(eten, "range", 2, comp.def[kind].tendon.spec.range, text); + ReadAttr(eten, "margin", 1, &comp.def[kind].tendon.spec.margin, text); + ReadAttr(eten, "stiffness", 1, &comp.def[kind].tendon.spec.stiffness, text); + ReadAttr(eten, "damping", 1, &comp.def[kind].tendon.spec.damping, text); + ReadAttr(eten, "frictionloss", 1, &comp.def[kind].tendon.spec.frictionloss, text); + ReadAttrTxt(eten, "material", material); + mjm_setString(comp.def[kind].tendon.spec.material, material.c_str()); + ReadAttr(eten, "rgba", 4, comp.def[kind].tendon.spec.rgba, text); + ReadAttr(eten, "width", 1, &comp.def[kind].tendon.spec.width, text); // advance eten = eten->NextSiblingElement("tendon"); @@ -2417,8 +2441,8 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { if (MapValue(edge, "equality", &n, bool_map, 2)) { fcomp.equality = (n==1); } - ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.solref, text, false, false); - ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.solimp, text, false, false); + ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.spec.solref, text, false, false); + ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.spec.solimp, text, false, false); ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text); ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text); } @@ -2577,10 +2601,10 @@ void mjXReader::Default(XMLElement* section, int parentid) { else if (name=="pair") OnePair(elem, &def->pair); // read equality - else if (name=="equality") OneEquality(elem, &def->equality); + else if (name=="equality") OneEquality(elem, &def->equality.spec); // read tendon - else if (name=="tendon") OneTendon(elem, &def->tendon); + else if (name=="tendon") OneTendon(elem, &def->tendon.spec); // read actuator else if (name=="general" || @@ -2602,6 +2626,8 @@ void mjXReader::Default(XMLElement* section, int parentid) { mjm_finalize(def->camera.spec.element); mjm_finalize(def->light.spec.element); mjm_finalize(def->actuator.spec.element); + mjm_finalize(def->equality.spec.element); + mjm_finalize(def->tendon.spec.element); // advance elem = elem->NextSiblingElement(); @@ -3318,7 +3344,7 @@ void mjXReader::Equality(XMLElement* section) { } // create equality constraint and parse - mjCEquality* pequality = model->AddEquality(def); + mjmEquality* pequality = mjm_addEquality(model, def); OneEquality(elem, pequality); // advance to next element @@ -3382,7 +3408,7 @@ void mjXReader::Tendon(XMLElement* section) { } // create equality constraint and parse - mjCTendon* pten = model->AddTendon(def); + mjmTendon* pten = mjm_addTendon(model, def); OneTendon(elem, pten); // process wrap sub-elements @@ -3390,11 +3416,12 @@ void mjXReader::Tendon(XMLElement* section) { while (sub) { // get wrap type string wrap = sub->Value(); + mjmWrap* pwrap;; // read attributes depending on type if (wrap=="site") { ReadAttrTxt(sub, "site", text, true); - pten->WrapSite(text, "line = " + std::to_string(sub->GetLineNum())); + pwrap = mjm_wrapSite(pten, text.c_str()); } else if (wrap=="geom") { @@ -3402,24 +3429,26 @@ void mjXReader::Tendon(XMLElement* section) { if (!ReadAttrTxt(sub, "sidesite", text1)) { text1.clear(); } - pten->WrapGeom(text, text1, "line = " + std::to_string(sub->GetLineNum())); + pwrap = mjm_wrapGeom(pten, text.c_str(), text1.c_str()); } else if (wrap=="pulley") { ReadAttr(sub, "divisor", 1, &data, text, true); - pten->WrapPulley(data, "line = " + std::to_string(sub->GetLineNum())); + pwrap = mjm_wrapPulley(pten, data); } else if (wrap=="joint") { ReadAttrTxt(sub, "joint", text, true); ReadAttr(sub, "coef", 1, &data, text1, true); - pten->WrapJoint(text, data, "line = " + std::to_string(sub->GetLineNum())); + pwrap = mjm_wrapJoint(pten, text.c_str(), data); } else { throw mjXError(sub, "unknown wrap type"); // SHOULD NOT OCCUR } + mjm_setString(pwrap->info, ("line = " + std::to_string(sub->GetLineNum())).c_str()); + // advance to next sub-element sub = sub->NextSiblingElement(); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index ce69a21b..5f4dc6c1 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -63,8 +63,8 @@ class mjXReader : public mjXBase { void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera); void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight); void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair); - void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality); - void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon); + void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality); + void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon); void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator); void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjCDef* def); void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 408dcd1a..f089ef28 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -559,33 +559,33 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) { switch (peq->type) { case mjEQ_CONNECT: - WriteAttrTxt(elem, "body1", peq->name1); - WriteAttrTxt(elem, "body2", peq->name2); + WriteAttrTxt(elem, "body1", mjm_getString(peq->name1)); + WriteAttrTxt(elem, "body2", mjm_getString(peq->name2)); WriteAttr(elem, "anchor", 3, peq->data); break; case mjEQ_WELD: - WriteAttrTxt(elem, "body1", peq->name1); - WriteAttrTxt(elem, "body2", peq->name2); + WriteAttrTxt(elem, "body1", mjm_getString(peq->name1)); + WriteAttrTxt(elem, "body2", mjm_getString(peq->name2)); WriteAttr(elem, "anchor", 3, peq->data); WriteAttr(elem, "torquescale", 1, peq->data+10); WriteAttr(elem, "relpose", 7, peq->data+3); break; case mjEQ_JOINT: - WriteAttrTxt(elem, "joint1", peq->name1); - WriteAttrTxt(elem, "joint2", peq->name2); + WriteAttrTxt(elem, "joint1", mjm_getString(peq->name1)); + WriteAttrTxt(elem, "joint2", mjm_getString(peq->name2)); WriteAttr(elem, "polycoef", 5, peq->data); break; case mjEQ_TENDON: - WriteAttrTxt(elem, "tendon1", peq->name1); - WriteAttrTxt(elem, "tendon2", peq->name2); + WriteAttrTxt(elem, "tendon1", mjm_getString(peq->name1)); + WriteAttrTxt(elem, "tendon2", mjm_getString(peq->name2)); WriteAttr(elem, "polycoef", 5, peq->data); break; case mjEQ_FLEX: - WriteAttrTxt(elem, "flex", peq->name1); + WriteAttrTxt(elem, "flex", mjm_getString(peq->name1)); break; default: @@ -646,9 +646,9 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) { // userdata if (writingdefaults) { - WriteVector(elem, "user", pten->userdata); + WriteVector(elem, "user", pten->get_userdata()); } else { - WriteVector(elem, "user", pten->userdata, def->tendon.userdata); + WriteVector(elem, "user", pten->get_userdata(), def->tendon.get_userdata()); } }