diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index db09b8fb..c6c5d9de 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -2120,6 +2120,14 @@ void mjCSkin::PointToLocal() { +void mjCSkin::NameSpace(const mjCModel* m) { + for (int i=0; i<(int)spec_bodyname_.size(); i++) { + spec_bodyname_[i] = m->prefix + spec_bodyname_[i] + m->suffix; + } +} + + + void mjCSkin::CopyFromSpec() { *static_cast(this) = spec; file_ = spec_file_; @@ -2163,6 +2171,20 @@ mjCSkin::~mjCSkin() { +void mjCSkin::ResolveReferences(const mjCModel* m) { + size_t nbone = bodyname_.size(); + bodyid.resize(nbone); + for (int i=0; iFindObject(mjOBJ_BODY, bodyname_[i]); + if (!pbody) { + throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str()); + } + bodyid[i] = pbody->id; + } +} + + + // compiler void mjCSkin::Compile(const mjVFS* vfs) { CopyFromSpec(); @@ -2243,14 +2265,7 @@ void mjCSkin::Compile(const mjVFS* vfs) { } // resolve body names - bodyid.resize(nbone); - for (int i=0; iFindObject(mjOBJ_BODY, bodyname_[i]); - if (!pbody) { - throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str()); - } - bodyid[i] = pbody->id; - } + ResolveReferences(model); // resolve material name mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material_); @@ -2510,6 +2525,15 @@ void mjCFlex::PointToLocal() { } + +void mjCFlex::NameSpace(const mjCModel* m) { + for (int i=0; i<(int)spec_vertbody_.size(); i++) { + spec_vertbody_[i] = m->prefix + spec_vertbody_[i] + m->suffix; + } +} + + + void mjCFlex::CopyFromSpec() { *static_cast(this) = spec; spec.info = (mjString)&info; @@ -2542,6 +2566,18 @@ void mjCFlex::DelTexcoord() { } +void mjCFlex::ResolveReferences(const mjCModel* m) { + for (int i=0; i<(int)vertbody_.size(); i++) { + mjCBase* pbody = m->FindObject(mjOBJ_BODY, vertbody_[i]); + if (pbody) { + vertbodyid.push_back(pbody->id); + } else { + throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str()); + } + } +} + + // compiler void mjCFlex::Compile(const mjVFS* vfs) { CopyFromSpec(); @@ -2603,14 +2639,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // resolve body ids - for (int i=0; i<(int)vertbody_.size(); i++) { - mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody_[i]); - if (pbody) { - vertbodyid.push_back(pbody->id); - } else { - throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str()); - } - } + ResolveReferences(model); // process elements for (int e=0; e<(int)elem_.size()/(dim+1); e++) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8728b5e6..0cd073f6 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -120,6 +120,8 @@ mjCModel::mjCModel() { tuples.clear(); keys.clear(); defaults.clear(); + prefix = ""; + suffix = ""; Clear(); //------------------------ master default set @@ -180,101 +182,66 @@ mjCModel& mjCModel::operator=(const mjCModel& other) { +// copy vector of elements from another model to this model +template +void mjCModel::CopyList(std::vector& dest, + const std::vector& source, + std::map& def_map, + const std::vector& defaults) { + // loop over the elements from the other model + for (T* element : source) { + try { + // try to find the referenced object in this model + element->NameSpace(element->model); + element->CopyFromSpec(); + element->ResolveReferences(this); + } catch (mjCError err) { + // if not present, skip the element + continue; + } + // copy the element from the other model to this model + dest.push_back(new T(*element)); + dest.back()->model = this; + dest.back()->def = defaults[def_map[element->def]]; + } +} + + + mjCModel& mjCModel::operator+=(const mjCModel& other) { if (this != &other) { + // create global lists + MakeLists(bodies[0]); + CreateObjectLists(); + ProcessLists(); + + // copy all elements not in the tree std::map def_map; for (int i = 0; i < other.defaults.size(); i++) { defaults.push_back(new mjCDef(*other.defaults[i])); def_map[other.defaults[i]] = i; } - for (const mjCFlex* flex : other.flexes) { - flexes.push_back(new mjCFlex(*flex)); - flexes.back()->model = this; - flexes.back()->def = defaults[def_map[flex->def]]; - } - for (const mjCMesh* mesh : other.meshes) { - meshes.push_back(new mjCMesh(*mesh)); - meshes.back()->model = this; - meshes.back()->def = defaults[def_map[mesh->def]]; - } - for (const mjCSkin* skin : other.skins) { - skins.push_back(new mjCSkin(*skin)); - skins.back()->model = this; - skins.back()->def = defaults[def_map[skin->def]]; - } - for (const mjCHField* hfield : other.hfields) { - hfields.push_back(new mjCHField(*hfield)); - hfields.back()->model = this; - hfields.back()->def = defaults[def_map[hfield->def]]; - } - for (const mjCTexture* texture : other.textures) { - textures.push_back(new mjCTexture(*texture)); - textures.back()->model = this; - textures.back()->def = defaults[def_map[texture->def]]; - } - for (const mjCMaterial* material : other.materials) { - materials.push_back(new mjCMaterial(*material)); - materials.back()->model = this; - materials.back()->def = defaults[def_map[material->def]]; - } - for (const mjCPair* pair : other.pairs) { - pairs.push_back(new mjCPair(*pair)); - pairs.back()->model = this; - pairs.back()->def = defaults[def_map[pair->def]]; - } - for (const mjCBodyPair* exclude : other.excludes) { - excludes.push_back(new mjCBodyPair(*exclude)); - excludes.back()->model = this; - excludes.back()->def = defaults[def_map[exclude->def]]; - } - for (const mjCEquality* equality : other.equalities) { - equalities.push_back(new mjCEquality(*equality)); - equalities.back()->model = this; - equalities.back()->def = defaults[def_map[equality->def]]; - } - for (const mjCTendon* tendon : other.tendons) { - tendons.push_back(new mjCTendon(*tendon)); - tendons.back()->SetModel(this); - tendons.back()->def = defaults[def_map[tendon->def]]; - } - for (const mjCActuator* actuator : other.actuators) { - actuators.push_back(new mjCActuator(*actuator)); - actuators.back()->model = this; - actuators.back()->def = defaults[def_map[actuator->def]]; - } - for (const mjCSensor* sensor : other.sensors) { - sensors.push_back(new mjCSensor(*sensor)); - sensors.back()->model = this; - sensors.back()->def = defaults[def_map[sensor->def]]; - } - for (const mjCNumeric* numeric : other.numerics) { - numerics.push_back(new mjCNumeric(*numeric)); - numerics.back()->model = this; - numerics.back()->def = defaults[def_map[numeric->def]]; - } - for (const mjCText* text : other.texts) { - texts.push_back(new mjCText(*text)); - texts.back()->model = this; - texts.back()->def = defaults[def_map[text->def]]; - } - for (const mjCTuple* tuple : other.tuples) { - tuples.push_back(new mjCTuple(*tuple)); - tuples.back()->model = this; - tuples.back()->def = defaults[def_map[tuple->def]]; - } - for (const mjCKey* key : other.keys) { - keys.push_back(new mjCKey(*key)); - keys.back()->model = this; - keys.back()->def = defaults[def_map[key->def]]; - } + CopyList(flexes, other.flexes, def_map, defaults); + CopyList(meshes, other.meshes, def_map, defaults); + CopyList(skins, other.skins, def_map, defaults); + CopyList(hfields, other.hfields, def_map, defaults); + CopyList(textures, other.textures, def_map, defaults); + CopyList(materials, other.materials, def_map, defaults); + CopyList(pairs, other.pairs, def_map, defaults); + CopyList(excludes, other.excludes, def_map, defaults); + CopyList(tendons, other.tendons, def_map, defaults); + CopyList(equalities, other.equalities, def_map, defaults); + CopyList(actuators, other.actuators, def_map, defaults); + CopyList(sensors, other.sensors, def_map, defaults); + CopyList(numerics, other.numerics, def_map, defaults); + CopyList(texts, other.texts, def_map, defaults); + CopyList(tuples, other.tuples, def_map, defaults); + CopyList(keys, other.keys, def_map, defaults); // plugins are global plugins = other.plugins; active_plugins = other.active_plugins; - // create global lists - MakeLists(bodies[0]); - // update defaults for the copied objects for (int i = 1; i < other.bodies.size(); i++) { bodies[i]->def = defaults[def_map[other.bodies[i]->def]]; @@ -296,7 +263,7 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) { } // cast children to mjCBase - CreateObjectLists(); + // restore to the same state as other if (!compiled) { @@ -670,28 +637,28 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) { //------------------------ API FOR ACCESS TO PRIVATE VARIABLES ------------------------------------- // compiled flag -bool mjCModel::IsCompiled(void) { +bool mjCModel::IsCompiled(void) const { return compiled; } // get reference of error object -const mjCError& mjCModel::GetError(void) { +const mjCError& mjCModel::GetError(void) const { return errInfo; } // pointer to world body -mjCBody* mjCModel::GetWorld(void) { +mjCBody* mjCModel::GetWorld(void) const { return bodies[0]; } // find default class name in array -mjCDef* mjCModel::FindDef(string name) { +mjCDef* mjCModel::FindDef(string name) const { for (int i=0; i<(int)defaults.size(); i++) { if (defaults[i]->name==name) { return defaults[i]; @@ -754,7 +721,7 @@ static T* findobject(std::string_view name, const vector& list, const mjKeyM } // find object in global lists given string type and name -mjCBase* mjCModel::FindObject(mjtObj type, string name) { +mjCBase* mjCModel::FindObject(mjtObj type, string name) const { if (!object_lists[type]) { return nullptr; } @@ -764,7 +731,7 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) { // find body by name -mjCBody* mjCModel::FindBody(mjCBody* body, std::string name) { +mjCBody* mjCModel::FindBody(mjCBody* body, std::string name) const { if (body->name == name) { return body; } @@ -782,7 +749,7 @@ mjCBody* mjCModel::FindBody(mjCBody* body, std::string name) { // find frame by name -mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) { +mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) const{ for (auto frame : body->frames) { if (frame->name == name) { return frame; @@ -802,7 +769,7 @@ mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) { // detect null pose -bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) { +bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) const { bool result = true; // check position if given @@ -2913,6 +2880,21 @@ static void processlist(mjListKeyMap& ids, vector& list, } + +// set object ids, check for repeated names +void mjCModel::ProcessLists() { + for (int i = 0; i < mjNOBJECT; i++) { + if (i != mjOBJ_XBODY && object_lists[i]) { + processlist(ids, *object_lists[i], (mjtObj) i); + } + } + + // check repeated names in meta elements + processlist(ids, frames, mjOBJ_FRAME); +} + + + // error handler for low-level engine static thread_local std::jmp_buf error_jmp_buf; static thread_local char errortext[500] = ""; @@ -3046,14 +3028,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { CheckEmptyNames(); // set object ids, check for repeated names - for (int i = 0; i < mjNOBJECT; i++) { - if (i != mjOBJ_XBODY && object_lists[i]) { - processlist(ids, *object_lists[i], (mjtObj) i); - } - } - - // check repeated names in meta elements - processlist(ids, frames, mjOBJ_FRAME); + ProcessLists(); // delete visual assets if (discardvisual) { diff --git a/src/user/user_model.h b/src/user/user_model.h index d6c1933f..167d3840 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -34,6 +34,11 @@ typedef std::map > mjKeyMap; typedef std::array mjListKeyMap; class mjCModel_ { + public: + // attach namespaces + std::string prefix; + std::string suffix; + protected: bool compiled; // already compiled flag @@ -185,6 +190,12 @@ class mjCModel : public mjCModel_, private mjSpec { mjCKey* AddKey(); mjCPlugin* AddPlugin(); + // copy vector of elements to this model + template void CopyList(std::vector& dest, + const std::vector& source, + std::map& def_map, + const std::vector& defaults); + // delete elements marked as discard=true template void Delete(std::vector& elements, const std::vector& discard); @@ -197,15 +208,15 @@ class mjCModel : public mjCModel_, private mjSpec { mjCBase* GetObject(mjtObj type, int id); // pointer to specified object // API for access to other variables - bool IsCompiled(); // is model already compiled - const mjCError& GetError(void); // get reference of error object - mjCBody* GetWorld(); // pointer to world body - mjCDef* FindDef(std::string name); // find default class name - mjCDef* AddDef(std::string name, int parentid); // add default class to array - mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name - mjCBody* FindBody(mjCBody* body, std::string name); // find body given name - mjCFrame* FindFrame(mjCBody* body, std::string name); // find frame given name - bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose + bool IsCompiled() const; // is model already compiled + const mjCError& GetError(void) const; // get reference of error object + mjCBody* GetWorld() const; // pointer to world body + mjCDef* FindDef(std::string name) const; // find default class name + mjCDef* AddDef(std::string name, int parentid); // add default class to array + mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name + mjCBody* FindBody(mjCBody* body, std::string name) const; // find body given name + mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name + bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose // accessors std::string get_meshdir(void) const { return meshdir_; } @@ -289,6 +300,9 @@ class mjCModel : public mjCModel_, private mjSpec { // create mjCBase lists from children lists void CreateObjectLists(); + // populate objects ids + void ProcessLists(); + mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info bool plugin_owner; // this class allocated the plugins diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e4f3c744..8e2a1d78 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -793,15 +793,45 @@ mjCBody::~mjCBody() { // apply prefix and suffix, propagate to children -void mjCBody::SetNameSpace() { +void mjCBody::NameSpace(const mjCModel* m) { if (!name.empty()) { name = prefix + name + suffix; } + for (auto& joint : joints) { + if (!joint->name.empty()) { + joint->name = prefix + joint->name + suffix; + } + } + + for (auto& geom : geoms) { + if (!geom->name.empty()) { + geom->name = prefix + geom->name + suffix; + } + } + + for (auto& site : sites) { + if (!site->name.empty()) { + site->name = prefix + site->name + suffix; + } + } + + for (auto& camera : cameras) { + if (!camera->name.empty()) { + camera->name = prefix + camera->name + suffix; + } + } + + for (auto& light : lights) { + if (!light->name.empty()) { + light->name = prefix + light->name + suffix; + } + } + for (auto& body : bodies) { - body->prefix = prefix; - body->suffix = suffix; - body->SetNameSpace(); + body->prefix = m->prefix; + body->suffix = m->suffix; + body->NameSpace(m); } } @@ -1368,8 +1398,10 @@ mjCFrame& mjCFrame::operator=(const mjCFrame& other) { // attach body to frame mjCFrame& mjCFrame::operator+=(const mjCBody& other) { mjCBody* subtree = new mjCBody(other, model); + other.model->prefix = subtree->prefix; + other.model->suffix = subtree->suffix; subtree->SetFrame(this); - subtree->SetNameSpace(); + subtree->NameSpace(other.model); // add to tree *body += *subtree; @@ -3777,8 +3809,8 @@ mjCPair& mjCPair::operator=(const mjCPair& other) { this->spec = other.spec; *static_cast(this) = static_cast(other); *static_cast(this) = static_cast(other); - this->geom1 = other.geom1; - this->geom2 = other.geom2; + this->geom1 = nullptr; + this->geom2 = nullptr; } PointToLocal(); return *this; @@ -3797,6 +3829,16 @@ void mjCPair::PointToLocal() { +void mjCPair::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + spec_geomname1_ = m->prefix + spec_geomname1_ + m->suffix; + spec_geomname2_ = m->prefix + spec_geomname2_ + m->suffix; +} + + + void mjCPair::CopyFromSpec() { *static_cast(this) = spec; geomname1_ = spec_geomname1_; @@ -3807,31 +3849,19 @@ void mjCPair::CopyFromSpec() { -// 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); - } - +void mjCPair::ResolveReferences(const mjCModel* m) { // find geom 1 - geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1_); + geom1 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname1_); if (!geom1) { 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*)m->FindObject(mjOBJ_GEOM, geomname2_); if (!geom2) { throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id); } - // mark geoms as not visual - geom1->SetNotVisual(); - geom2->SetNotVisual(); - // swap if body1 > body2 if (geom1->body->id > geom2->body->id) { string nametmp = geomname1_; @@ -3845,6 +3875,25 @@ void mjCPair::Compile(void) { // get geom ids and body signature signature = ((geom1->body->id)<<16) + geom2->body->id; +} + + + +// 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 geoms + ResolveReferences(model); + + // mark geoms as not visual + geom1->SetNotVisual(); + geom2->SetNotVisual(); // set undefined margin: max if (!mjuu_defined(margin)) { @@ -3989,6 +4038,16 @@ void mjCBodyPair::PointToLocal() { +void mjCBodyPair::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + spec_bodyname1_ = m->prefix + spec_bodyname1_ + m->suffix; + spec_bodyname2_ = m->prefix + spec_bodyname2_ + m->suffix; +} + + + void mjCBodyPair::CopyFromSpec() { *static_cast(this) = spec; bodyname1_ = spec_bodyname1_; @@ -3999,18 +4058,15 @@ void mjCBodyPair::CopyFromSpec() { -// compiler -void mjCBodyPair::Compile(void) { - CopyFromSpec(); - +void mjCBodyPair::ResolveReferences(const mjCModel* m) { // find body 1 - mjCBody* pb1 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname1_); + mjCBody* pb1 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname1_); if (!pb1) { 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*)m->FindObject(mjOBJ_BODY, bodyname2_); if (!pb2) { throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2_.c_str(), id); } @@ -4034,6 +4090,16 @@ void mjCBodyPair::Compile(void) { +// compiler +void mjCBodyPair::Compile(void) { + CopyFromSpec(); + + // find bodies + ResolveReferences(model); +} + + + //------------------ class mjCEquality implementation ---------------------------------------------- // initialize default constraint @@ -4091,6 +4157,17 @@ void mjCEquality::PointToLocal() { } + +void mjCEquality::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + spec_name1_ = m->prefix + spec_name1_ + m->suffix; + spec_name2_ = m->prefix + spec_name2_ + m->suffix; +} + + + void mjCEquality::CopyFromSpec() { *static_cast(this) = spec; name1_ = spec_name1_; @@ -4100,10 +4177,8 @@ void mjCEquality::CopyFromSpec() { } -// compiler -void mjCEquality::Compile(void) { - CopyFromSpec(); +void mjCEquality::ResolveReferences(const mjCModel* m) { mjtObj objtype; mjCBase *px1, *px2; mjtJoint jt1, jt2; @@ -4122,7 +4197,7 @@ void mjCEquality::Compile(void) { } // find object 1, get id - px1 = model->FindObject(objtype, name1_); + px1 = m->FindObject(objtype, name1_); if (!px1) { throw mjCError(this, "unknown element '%s' in equality constraint %d", name1_.c_str(), id); } @@ -4130,7 +4205,7 @@ void mjCEquality::Compile(void) { // find object 2, get id if (!name2_.empty()) { - px2 = model->FindObject(objtype, name2_); + px2 = m->FindObject(objtype, name2_); if (!px2) { throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id); } @@ -4153,11 +4228,6 @@ void mjCEquality::Compile(void) { obj2id = 0; } - // 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); - } - // 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); @@ -4177,6 +4247,21 @@ void mjCEquality::Compile(void) { +// compiler +void mjCEquality::Compile(void) { + CopyFromSpec(); + + // find objects + ResolveReferences(model); + + // 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); + } +} + + + //------------------ class mjCTendon implementation ------------------------------------------------ // constructor @@ -4243,6 +4328,17 @@ void mjCTendon::PointToLocal() { +void mjCTendon::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + for (int i=0; iNameSpace(model); + } +} + + + void mjCTendon::CopyFromSpec() { *static_cast(this) = spec; material_ = spec_material_; @@ -4361,6 +4457,14 @@ mjCWrap* mjCTendon::GetWrap(int id) { +void mjCTendon::ResolveReferences(const mjCModel* m) { + for (int i=0; iResolveReferences(m); + } +} + + + // compiler void mjCTendon::Compile(void) { CopyFromSpec(); @@ -4395,9 +4499,7 @@ void mjCTendon::Compile(void) { } // compile objects in path - for (int i=0; iCompile(); - } + ResolveReferences(model); // check path for (int i=0; iprefix + name + m->suffix; + sidesite = m->prefix + sidesite + m->suffix; +} + + + +void mjCWrap::ResolveReferences(const mjCModel* m) { mjCBase *pside; // handle wrap object types switch (type) { case mjWRAP_JOINT: // joint // find joint by name - obj = model->FindObject(mjOBJ_JOINT, name); + obj = m->FindObject(mjOBJ_JOINT, name); if (!obj) { throw mjCError(this, "joint '%s' not found in tendon %d, wrap %d", @@ -4555,7 +4663,7 @@ void mjCWrap::Compile(void) { case mjWRAP_SPHERE: // geom (cylinder type set here) // find geom by name - obj = model->FindObject(mjOBJ_GEOM, name); + obj = m->FindObject(mjOBJ_GEOM, name); if (!obj) { throw mjCError(this, "geom '%s' not found in tendon %d, wrap %d", @@ -4574,7 +4682,7 @@ void mjCWrap::Compile(void) { // process side site if (!sidesite.empty()) { // find site by name - pside = model->FindObject(mjOBJ_SITE, sidesite); + pside = m->FindObject(mjOBJ_SITE, sidesite); if (!pside) { throw mjCError(this, "side site '%s' not found in tendon %d, wrap %d", @@ -4598,7 +4706,7 @@ void mjCWrap::Compile(void) { case mjWRAP_SITE: // site // find site by name - obj = model->FindObject(mjOBJ_SITE, name); + obj = m->FindObject(mjOBJ_SITE, name); if (!obj) { throw mjCError(this, "site '%s' not found in wrap %d", name.c_str(), id); } @@ -4618,6 +4726,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { mjm_defaultActuator(spec); // clear private variables + ptarget = nullptr; spec_target_.clear(); spec_slidersite_.clear(); spec_refsite_.clear(); @@ -4653,6 +4762,7 @@ mjCActuator& mjCActuator::operator=(const mjCActuator& other) { this->spec = other.spec; *static_cast(this) = static_cast(other); *static_cast(this) = static_cast(other); + ptarget = nullptr; } PointToLocal(); return *this; @@ -4681,6 +4791,17 @@ void mjCActuator::PointToLocal() { +void mjCActuator::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + spec_target_ = m->prefix + spec_target_ + m->suffix; + spec_refsite_ = m->prefix + spec_refsite_ + m->suffix; + spec_slidersite_ = m->prefix + spec_slidersite_ + m->suffix; +} + + + void mjCActuator::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; @@ -4699,31 +4820,13 @@ void mjCActuator::CopyFromSpec() { -// compiler -void mjCActuator::Compile(void) { - CopyFromSpec(); +void mjCActuator::ResolveReferences(const mjCModel* m) { mjCJoint* pjnt; - - // resize userdata - if (userdata_.size() > model->nuser_actuator) { - throw mjCError(this, "user has more values than nuser_actuator in actuator '%s' (id = %d)", - name.c_str(), id); - } - userdata_.resize(model->nuser_actuator); - - // check for missing target name - if (target_.empty()) { - throw mjCError(this, - "missing transmission target for actuator '%s' (id = %d)", name.c_str(), id); - } - - // find transmission target in object arrays - mjCBase* ptarget = 0; switch (trntype) { case mjTRN_JOINT: case mjTRN_JOINTINPARENT: // get joint - ptarget = model->FindObject(mjOBJ_JOINT, target_); + ptarget = m->FindObject(mjOBJ_JOINT, target_); if (!ptarget) { throw mjCError(this, "unknown transmission target '%s' for actuator id = %d", target_.c_str(), id); @@ -4743,7 +4846,7 @@ void mjCActuator::Compile(void) { if (slidersite_.empty()) { throw mjCError(this, "missing base site for slider-crank '%s' (id = %d)", name.c_str(), id); } - ptarget = model->FindObject(mjOBJ_SITE, slidersite_); + ptarget = m->FindObject(mjOBJ_SITE, slidersite_); if (!ptarget) { throw mjCError(this, "base site '%s' not found for actuator %d", slidersite_.c_str(), id); } @@ -4756,18 +4859,18 @@ void mjCActuator::Compile(void) { } // proceed with regular target - ptarget = model->FindObject(mjOBJ_SITE, target_); + ptarget = m->FindObject(mjOBJ_SITE, target_); break; case mjTRN_TENDON: // get tendon - ptarget = model->FindObject(mjOBJ_TENDON, target_); + ptarget = m->FindObject(mjOBJ_TENDON, target_); break; case mjTRN_SITE: // get refsite, copy into trnid[1] if (!refsite_.empty()) { - ptarget = model->FindObject(mjOBJ_SITE, refsite_); + ptarget = m->FindObject(mjOBJ_SITE, refsite_); if (!ptarget) { throw mjCError(this, "reference site '%s' not found for actuator %d", refsite_.c_str(), id); } @@ -4775,12 +4878,12 @@ void mjCActuator::Compile(void) { } // proceed with regular site target - ptarget = model->FindObject(mjOBJ_SITE, target_); + ptarget = m->FindObject(mjOBJ_SITE, target_); break; case mjTRN_BODY: // get body - ptarget = model->FindObject(mjOBJ_BODY, target_); + ptarget = m->FindObject(mjOBJ_BODY, target_); break; default: @@ -4793,6 +4896,29 @@ void mjCActuator::Compile(void) { } else { trnid[0] = ptarget->id; } +} + + + +// compiler +void mjCActuator::Compile(void) { + CopyFromSpec(); + + // resize userdata + if (userdata_.size() > model->nuser_actuator) { + throw mjCError(this, "user has more values than nuser_actuator in actuator '%s' (id = %d)", + name.c_str(), id); + } + userdata_.resize(model->nuser_actuator); + + // check for missing target name + if (target_.empty()) { + throw mjCError(this, + "missing transmission target for actuator '%s' (id = %d)", name.c_str(), id); + } + + // find transmission target in object arrays + ResolveReferences(model); // handle inheritrange if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE && @@ -4812,7 +4938,7 @@ void mjCActuator::Compile(void) { const double* target_range; if (trntype == mjTRN_JOINT) { - pjnt = (mjCJoint*) ptarget; + mjCJoint* pjnt = (mjCJoint*) ptarget; if (pjnt->spec.type != mjJNT_HINGE && pjnt->spec.type != mjJNT_SLIDE) { throw mjCError(this, "inheritrange can only be used with hinge and slide joints, " "actuator '%s' (id = %d)", name.c_str(), id); @@ -4950,6 +5076,7 @@ mjCSensor::mjCSensor(mjCModel* _model) { spec_refname_.clear(); spec_userdata_.clear(); obj = nullptr; + ref = nullptr; refid = -1; // in case this sensor is not compiled @@ -4972,6 +5099,8 @@ mjCSensor& mjCSensor::operator=(const mjCSensor& other) { this->spec = other.spec; *static_cast(this) = static_cast(other); *static_cast(this) = static_cast(other); + obj = nullptr; + ref = nullptr; } PointToLocal(); return *this; @@ -4993,6 +5122,16 @@ void mjCSensor::PointToLocal() { +void mjCSensor::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + spec_objname_ = m->prefix + spec_objname_ + m->suffix; + spec_refname_ = m->prefix + spec_refname_ + m->suffix; +} + + + void mjCSensor::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; @@ -5009,27 +5148,7 @@ void mjCSensor::CopyFromSpec() { -// compiler -void mjCSensor::Compile(void) { - CopyFromSpec(); - - // resize userdata - if (userdata_.size() > model->nuser_sensor) { - throw mjCError(this, "user has more values than nuser_sensor in sensor '%s' (id = %d)", - name.c_str(), id); - } - userdata_.resize(model->nuser_sensor); - - // require non-negative noise - if (noise<0) { - throw mjCError(this, "negative noise in sensor '%s' (id = %d)", name.c_str(), id); - } - - // require non-negative cutoff - if (cutoff<0) { - throw mjCError(this, "negative cutoff in sensor '%s' (id = %d)", name.c_str(), id); - } - +void mjCSensor::ResolveReferences(const mjCModel* m) { // get objid from objtype and objname if (objtype!=mjOBJ_UNKNOWN) { // check for missing object name @@ -5040,7 +5159,7 @@ void mjCSensor::Compile(void) { } // find name - obj = model->FindObject(objtype, objname_); + obj = m->FindObject(objtype, objname_); if (!obj) { throw mjCError(this, "unrecognized name of sensorized object in sensor '%s' (id = %d)", @@ -5067,8 +5186,8 @@ void mjCSensor::Compile(void) { } // find name - mjCBase* pref = model->FindObject(reftype, refname_); - if (!pref) { + ref = m->FindObject(reftype, refname_); + if (!ref) { throw mjCError(this, "unrecognized name of reference frame object in sensor '%s' (id = %d)", name.c_str(), id); @@ -5083,8 +5202,35 @@ void mjCSensor::Compile(void) { } // get sensorized object id - refid = pref->id; + refid = ref->id; } +} + + + +// compiler +void mjCSensor::Compile(void) { + CopyFromSpec(); + + // resize userdata + if (userdata_.size() > model->nuser_sensor) { + throw mjCError(this, "user has more values than nuser_sensor in sensor '%s' (id = %d)", + name.c_str(), id); + } + userdata_.resize(model->nuser_sensor); + + // require non-negative noise + if (noise<0) { + throw mjCError(this, "negative noise in sensor '%s' (id = %d)", name.c_str(), id); + } + + // require non-negative cutoff + if (cutoff<0) { + throw mjCError(this, "negative cutoff in sensor '%s' (id = %d)", name.c_str(), id); + } + + // Find referenced object + ResolveReferences(model); // process according to sensor type switch (type) { @@ -5126,7 +5272,7 @@ void mjCSensor::Compile(void) { // check for camera resolution for camera projection sensor if (type==mjSENS_CAMPROJECTION) { - mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname_); + mjCCamera* camref = (mjCCamera*)ref; if (!camref->resolution[0] || !camref->resolution[1]) { throw mjCError(this, "camera projection sensor requires camera resolution '%s' (id = %d)", @@ -5619,6 +5765,17 @@ void mjCTuple::PointToLocal() { +void mjCTuple::NameSpace(const mjCModel* m) { + if (!name.empty()) { + name = m->prefix + name + m->suffix; + } + for (int i=0; iprefix + spec_objname_[i] + m->suffix; + } +} + + + void mjCTuple::CopyFromSpec() { *static_cast(this) = spec; objtype_ = spec_objtype_; @@ -5644,10 +5801,7 @@ mjCTuple::~mjCTuple() { -// compiler -void mjCTuple::Compile(void) { - CopyFromSpec(); - +void mjCTuple::ResolveReferences(const mjCModel* m) { // check for empty tuple if (objtype_.empty()) { throw mjCError(this, "tuple '%s' (id = %d) is empty", name.c_str(), id); @@ -5665,7 +5819,7 @@ void mjCTuple::Compile(void) { // find objects, fill in ids for (int i=0; iFindObject(objtype_[i], objname_[i]); + mjCBase* res = m->FindObject(objtype_[i], objname_[i]); if (!res) { throw mjCError(this, "unrecognized object '%s' in tuple %d", objname_[i].c_str(), id); } @@ -5682,6 +5836,14 @@ void mjCTuple::Compile(void) { +// compiler +void mjCTuple::Compile(void) { + CopyFromSpec(); + ResolveReferences(model); +} + + + //------------------ class mjCKey implementation --------------------------------------------------- // constructor diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 50ae9f2e..1a4cd0c6 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -198,6 +198,12 @@ class mjCBase : public mjCBase_ { // Copy spec into private attributes virtual void CopyFromSpec() {} + // Throws an error if any of the references is missing + virtual void ResolveReferences(const mjCModel* m) {} + + // Appends prefix and suffix to reference + virtual void NameSpace(const mjCModel* m) {} + // Copy assignment mjCBase& operator=(const mjCBase& other); @@ -284,7 +290,7 @@ class mjCBody : public mjCBody_, private mjmBody { mjCBase* FindObject(mjtObj type, std::string name, bool recursive = true); // Propagate suffix and prefix to the whole tree - void SetNameSpace(); + void NameSpace(const mjCModel* m); // set explicitinertial to true void MakeInertialExplicit(); @@ -682,6 +688,8 @@ class mjCFlex: public mjCFlex_, private mjmFlex { void CopyFromSpec(void); void PointToLocal(void); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); // used by mjXWriter and mjCModel const std::string& get_material() { return material_; } @@ -937,6 +945,8 @@ class mjCSkin: public mjCSkin_, private mjmSkin { mjCSkin& operator=(const mjCSkin& other); // copy assignment ~mjCSkin(); // destructor + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); void Compile(const mjVFS* vfs); // compiler void LoadSKN(mjResource* resource); // load skin in SKN BIN format }; @@ -1115,6 +1125,8 @@ class mjCPair : public mjCPair_, private mjmPair { void CopyFromSpec(); void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); std::string get_geomname1() { return geomname1_; } std::string get_geomname2() { return geomname2_; } @@ -1162,6 +1174,8 @@ class mjCBodyPair : public mjCBodyPair_, private mjmExclude { void CopyFromSpec(); void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); std::string get_bodyname1() const { return bodyname1_; } std::string get_bodyname2() const { return bodyname2_; } @@ -1207,6 +1221,8 @@ class mjCEquality : public mjCEquality_, private mjmEquality { void CopyFromSpec(); void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); private: mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor @@ -1264,6 +1280,8 @@ class mjCTendon : public mjCTendon_, private mjmTendon { void CopyFromSpec(); void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); void SetModel(mjCModel* _model); bool is_limited() const; @@ -1299,6 +1317,9 @@ class mjCWrap : public mjCWrap_, private mjmWrap { using mjCBase::info; void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); + mjCBase* obj; // wrap object pointer private: @@ -1306,8 +1327,6 @@ class mjCWrap : public mjCWrap_, private mjmWrap { mjCWrap(const mjCWrap& other); // copy constructor mjCWrap& operator=(const mjCWrap& other); // copy assignment - void Compile(void); // compiler - mjCTendon* tendon; // tendon owning this wrap }; @@ -1393,6 +1412,10 @@ class mjCActuator : public mjCActuator_, private mjmActuator { void Compile(void); // compiler void CopyFromSpec(); void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); + + mjCBase* ptarget; // transmission target }; @@ -1403,7 +1426,6 @@ class mjCActuator : public mjCActuator_, private mjmActuator { class mjCSensor_ : public mjCBase { protected: int refid; // id of reference frame - mjCBase* obj; // sensorized object // variable-size data std::string plugin_name; @@ -1440,6 +1462,11 @@ class mjCSensor : public mjCSensor_, private mjmSensor { void Compile(void); // compiler void CopyFromSpec(); void PointToLocal(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); + + mjCBase* obj; // sensorized object + mjCBase* ref; // sensorized reference }; @@ -1533,6 +1560,8 @@ class mjCTuple : public mjCTuple_, private mjmTuple { void PointToLocal(); void CopyFromSpec(); + void ResolveReferences(const mjCModel* m); + void NameSpace(const mjCModel* m); private: mjCTuple(mjCModel*); // constructor diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index d6aa0f87..e0a83d5f 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -215,13 +215,27 @@ TEST_F(MujocoTest, Attach) { static constexpr char xml_child[] = R"( - - + + + + + + + + + + + + + + + + )"; static constexpr char xml_result[] = R"( @@ -231,8 +245,8 @@ TEST_F(MujocoTest, Attach) { - - + + @@ -240,6 +254,18 @@ TEST_F(MujocoTest, Attach) { + + + + + + + + + + + + )"; // model with one free sphere and a frame @@ -255,7 +281,7 @@ TEST_F(MujocoTest, Attach) { EXPECT_THAT(child, NotNull()) << er.data(); // get subtree - mjmBody* body = mjm_findBody(child, "cylinder"); + mjmBody* body = mjm_findBody(child, "body"); EXPECT_THAT(body, NotNull()); // attach child to parent frame @@ -267,7 +293,7 @@ TEST_F(MujocoTest, Attach) { EXPECT_THAT(m_attached, NotNull()); // check full name stored in mjModel - EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-cylinder-1"); + EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1"); // check body 2 is attached to body 1 EXPECT_THAT(m_attached->body_parentid[2], 1);