From 8a9b041388941e394ec0939294d6e0fd8b05bc4c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 10 Jan 2024 10:19:44 -0800 Subject: [PATCH] Replace `objid` with `obj` pointers. This in preparation of adding functions to remove/add elements after compilation. PiperOrigin-RevId: 597286844 Change-Id: I940103ade986c8d057db8cc1e9a98c073ddfab98 --- src/user/user_model.cc | 6 ++--- src/user/user_objects.cc | 52 +++++++++++++++--------------------- src/user/user_objects.h | 6 ++--- src/xml/xml_native_writer.cc | 25 +++++++---------- 4 files changed, 37 insertions(+), 52 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 766650a4..ca4790f4 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2105,7 +2105,7 @@ void mjCModel::CopyObjects(mjModel* m) { // set wraps for (int j=0; j<(int)pte->path.size(); j++) { m->wrap_type[adr+j] = pte->path[j]->type; - m->wrap_objid[adr+j] = pte->path[j]->objid; + m->wrap_objid[adr+j] = pte->path[j]->obj ? pte->path[j]->obj->id : -1; m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->prm; if (pte->path[j]->type==mjWRAP_SPHERE || pte->path[j]->type==mjWRAP_CYLINDER) { m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->sideid; @@ -2160,7 +2160,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->sensor_datatype[i] = psen->datatype; m->sensor_needstage[i] = psen->needstage; m->sensor_objtype[i] = psen->objtype; - m->sensor_objid[i] = psen->objid; + m->sensor_objid[i] = psen->obj ? psen->obj->id : -1; m->sensor_reftype[i] = psen->reftype; m->sensor_refid[i] = psen->refid; m->sensor_dim[i] = psen->dim; @@ -2219,7 +2219,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->tuple_size[i] = (int)ptu->objtype.size(); for (int j=0; jtuple_size[i]; j++) { m->tuple_objtype[adr+j] = (int)ptu->objtype[j]; - m->tuple_objid[adr+j] = ptu->objid[j]; + m->tuple_objid[adr+j] = ptu->obj[j]->id; m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm[j]; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 74f8825e..b1143c1e 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3637,7 +3637,7 @@ void mjCTendon::Compile(void) { } // site cannot be repeated - if (itype==mjWRAP_SITE && path[i]->objid==path[i+1]->objid) { + if (itype==mjWRAP_SITE && path[i]->obj->id==path[i+1]->obj->id) { throw mjCError(this, "tendon '%s' (id = %d): site %d is repeated", name.c_str(), id, i); @@ -3698,7 +3698,7 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) { // clear variables type = mjWRAP_NONE; - objid = -1; + obj = nullptr; sideid = -1; prm = 0; sidesite.clear(); @@ -3708,14 +3708,14 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) { // compiler void mjCWrap::Compile(void) { - mjCBase *ptr = 0, *pside; + mjCBase *pside; // handle wrap object types switch (type) { case mjWRAP_JOINT: // joint // find joint by name - ptr = model->FindObject(mjOBJ_JOINT, name); - if (!ptr) { + obj = model->FindObject(mjOBJ_JOINT, name); + if (!obj) { throw mjCError(this, "joint '%s' not found in tendon %d, wrap %d", name.c_str(), tendon->id, id); @@ -3725,17 +3725,17 @@ void mjCWrap::Compile(void) { case mjWRAP_SPHERE: // geom (cylinder type set here) // find geom by name - ptr = model->FindObject(mjOBJ_GEOM, name); - if (!ptr) { + obj = model->FindObject(mjOBJ_GEOM, name); + if (!obj) { throw mjCError(this, "geom '%s' not found in tendon %d, wrap %d", name.c_str(), tendon->id, id); } // set/check geom type - if (((mjCGeom*)ptr)->type == mjGEOM_CYLINDER) { + if (((mjCGeom*)obj)->type == mjGEOM_CYLINDER) { type = mjWRAP_CYLINDER; - } else if (((mjCGeom*)ptr)->type != mjGEOM_SPHERE) { + } else if (((mjCGeom*)obj)->type != mjGEOM_SPHERE) { throw mjCError(this, "geom '%s' in tendon %d, wrap %d is not sphere or cylinder", name.c_str(), tendon->id, id); @@ -3768,8 +3768,8 @@ void mjCWrap::Compile(void) { case mjWRAP_SITE: // site // find site by name - ptr = model->FindObject(mjOBJ_SITE, name); - if (!ptr) { + obj = model->FindObject(mjOBJ_SITE, name); + if (!obj) { throw mjCError(this, "site '%s' not found in wrap %d", name.c_str(), id); } break; @@ -3777,11 +3777,6 @@ void mjCWrap::Compile(void) { default: // SHOULD NOT OCCUR throw mjCError(this, "unknown wrap type in tendon %d, wrap %d", 0, tendon->id, id); } - - // set object id - if (ptr) { - objid = ptr->id; - } } @@ -4053,7 +4048,7 @@ mjCSensor::mjCSensor(mjCModel* _model) { dim = 0; // clear private variables - objid = -1; + obj = nullptr; refid = -1; plugin_instance = nullptr; @@ -4065,8 +4060,6 @@ mjCSensor::mjCSensor(mjCModel* _model) { // compiler void mjCSensor::Compile(void) { - const mjCBase* pobj; - // resize userdata if (userdata.size() > model->nuser_sensor) { throw mjCError(this, "user has more values than nuser_sensor in sensor '%s' (id = %d)", @@ -4094,15 +4087,14 @@ void mjCSensor::Compile(void) { } // find name - pobj = model->FindObject(objtype, objname); - if (!pobj) { + obj = model->FindObject(objtype, objname); + if (!obj) { throw mjCError(this, "unrecognized name of sensorized object in sensor '%s' (id = %d)", name.c_str(), id); } // get sensorized object id - objid = pobj->id; } else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN && type != mjSENS_USER) { throw mjCError(this, "invalid type in sensor '%s' (id = %d)", name.c_str(), id); } @@ -4195,7 +4187,7 @@ void mjCSensor::Compile(void) { } // make sure joint is slide or hinge - if (((mjCJoint*)pobj)->type!=mjJNT_SLIDE && ((mjCJoint*)pobj)->type!=mjJNT_HINGE) { + if (((mjCJoint*)obj)->type!=mjJNT_SLIDE && ((mjCJoint*)obj)->type!=mjJNT_HINGE) { throw mjCError(this, "joint must be slide or hinge in sensor '%s' (id = %d)", name.c_str(), id); } @@ -4260,7 +4252,7 @@ void mjCSensor::Compile(void) { } // make sure joint is ball - if (((mjCJoint*)pobj)->type!=mjJNT_BALL) { + if (((mjCJoint*)obj)->type!=mjJNT_BALL) { throw mjCError(this, "joint must be ball in sensor '%s' (id = %d)", name.c_str(), id); } @@ -4287,7 +4279,7 @@ void mjCSensor::Compile(void) { } // make sure joint has limit - if (!((mjCJoint*)pobj)->limited) { + if (!((mjCJoint*)obj)->limited) { throw mjCError(this, "joint must be limited in sensor '%s' (id = %d)", name.c_str(), id); } @@ -4313,7 +4305,7 @@ void mjCSensor::Compile(void) { } // make sure tendon has limit - if (!((mjCTendon*)pobj)->limited) { + if (!((mjCTendon*)obj)->limited) { throw mjCError(this, "tendon must be limited in sensor '%s' (id = %d)", name.c_str(), id); } @@ -4535,7 +4527,7 @@ mjCTuple::mjCTuple(mjCModel* _model) { objtype.clear(); objname.clear(); objprm.clear(); - objid.clear(); + obj.clear(); } @@ -4545,7 +4537,7 @@ mjCTuple::~mjCTuple() { objtype.clear(); objname.clear(); objprm.clear(); - objid.clear(); + obj.clear(); } @@ -4564,7 +4556,7 @@ void mjCTuple::Compile(void) { } // resize objid to correct size - objid.resize(objtype.size()); + obj.resize(objtype.size()); // find objects, fill in ids for (int i=0; iid; + obj[i] = res; } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 9641626c..75f7f5cf 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1069,7 +1069,7 @@ class mjCWrap : public mjCBase { public: mjtWrap type; // wrap object type - int objid; // wrap object id (in array corresponding to type) + mjCBase* obj; // wrap object pointer int sideid; // side site id; -1 if not applicable double prm; // parameter: divisor, coefficient std::string sidesite; // name of side site @@ -1179,7 +1179,7 @@ class mjCSensor : public mjCBase { mjCSensor(mjCModel*); // constructor void Compile(void); // compiler - int objid; // id of sensorized object + mjCBase* obj; // sensorized object int refid; // id of reference frame }; @@ -1239,7 +1239,7 @@ class mjCTuple : public mjCBase { ~mjCTuple(); // destructor void Compile(void); // compiler - std::vector objid; // object ids + std::vector obj; // object pointers }; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 0e7d281f..926e7089 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1648,34 +1648,27 @@ void mjXWriter::Tendon(XMLElement* root) { OneTendon(elem, pten, pten->def); // write wraps - mjCBase* pobj; XMLElement* wrap; for (int j=0; jNumWraps(); j++) { mjCWrap* pw = pten->GetWrap(j); switch (pw->type) { case mjWRAP_JOINT: - if ((pobj = model->GetObject(mjOBJ_JOINT, pw->objid))) { - wrap = InsertEnd(elem, "joint"); - WriteAttrTxt(wrap, "joint", pobj->name); - WriteAttr(wrap, "coef", 1, &pw->prm); - } + wrap = InsertEnd(elem, "joint"); + WriteAttrTxt(wrap, "joint", pw->obj->name); + WriteAttr(wrap, "coef", 1, &pw->prm); break; case mjWRAP_SITE: - if ((pobj = model->GetObject(mjOBJ_SITE, pw->objid))) { - wrap = InsertEnd(elem, "site"); - WriteAttrTxt(wrap, "site", pobj->name); - } + wrap = InsertEnd(elem, "site"); + WriteAttrTxt(wrap, "site", pw->obj->name); break; case mjWRAP_SPHERE: case mjWRAP_CYLINDER: - if ((pobj = model->GetObject(mjOBJ_GEOM, pw->objid))) { - wrap = InsertEnd(elem, "geom"); - WriteAttrTxt(wrap, "geom", pobj->name); - if (!pw->sidesite.empty()) { - WriteAttrTxt(wrap, "sidesite", pw->sidesite); - } + wrap = InsertEnd(elem, "geom"); + WriteAttrTxt(wrap, "geom", pw->obj->name); + if (!pw->sidesite.empty()) { + WriteAttrTxt(wrap, "sidesite", pw->sidesite); } break;