Do not attach elements not referenced in the subtree.

PiperOrigin-RevId: 617849280
Change-Id: I391fe7c00f67d020d7394ddd31219d727b2eaa94
This commit is contained in:
Alessio Quaglino
2024-03-21 08:20:50 -07:00
committed by Copybara-Service
parent b0daccc234
commit c3fb8e7f35
6 changed files with 475 additions and 240 deletions
+45 -16
View File
@@ -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() { void mjCSkin::CopyFromSpec() {
*static_cast<mjmSkin*>(this) = spec; *static_cast<mjmSkin*>(this) = spec;
file_ = spec_file_; 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; i<nbone; i++) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, bodyname_[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str());
}
bodyid[i] = pbody->id;
}
}
// compiler // compiler
void mjCSkin::Compile(const mjVFS* vfs) { void mjCSkin::Compile(const mjVFS* vfs) {
CopyFromSpec(); CopyFromSpec();
@@ -2243,14 +2265,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
} }
// resolve body names // resolve body names
bodyid.resize(nbone); ResolveReferences(model);
for (int i=0; i<nbone; i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname_[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str());
}
bodyid[i] = pbody->id;
}
// resolve material name // resolve material name
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material_); 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() { void mjCFlex::CopyFromSpec() {
*static_cast<mjmFlex*>(this) = spec; *static_cast<mjmFlex*>(this) = spec;
spec.info = (mjString)&info; 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 // compiler
void mjCFlex::Compile(const mjVFS* vfs) { void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec(); CopyFromSpec();
@@ -2603,14 +2639,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
// resolve body ids // resolve body ids
for (int i=0; i<(int)vertbody_.size(); i++) { ResolveReferences(model);
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());
}
}
// process elements // process elements
for (int e=0; e<(int)elem_.size()/(dim+1); e++) { for (int e=0; e<(int)elem_.size()/(dim+1); e++) {
+75 -100
View File
@@ -120,6 +120,8 @@ mjCModel::mjCModel() {
tuples.clear(); tuples.clear();
keys.clear(); keys.clear();
defaults.clear(); defaults.clear();
prefix = "";
suffix = "";
Clear(); Clear();
//------------------------ master default set //------------------------ master default set
@@ -180,101 +182,66 @@ mjCModel& mjCModel::operator=(const mjCModel& other) {
// copy vector of elements from another model to this model
template <class T>
void mjCModel::CopyList(std::vector<T*>& dest,
const std::vector<T*>& source,
std::map<mjCDef*, int>& def_map,
const std::vector<mjCDef*>& 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) { mjCModel& mjCModel::operator+=(const mjCModel& other) {
if (this != &other) { if (this != &other) {
// create global lists
MakeLists(bodies[0]);
CreateObjectLists();
ProcessLists();
// copy all elements not in the tree
std::map<mjCDef*, int> def_map; std::map<mjCDef*, int> def_map;
for (int i = 0; i < other.defaults.size(); i++) { for (int i = 0; i < other.defaults.size(); i++) {
defaults.push_back(new mjCDef(*other.defaults[i])); defaults.push_back(new mjCDef(*other.defaults[i]));
def_map[other.defaults[i]] = i; def_map[other.defaults[i]] = i;
} }
for (const mjCFlex* flex : other.flexes) { CopyList(flexes, other.flexes, def_map, defaults);
flexes.push_back(new mjCFlex(*flex)); CopyList(meshes, other.meshes, def_map, defaults);
flexes.back()->model = this; CopyList(skins, other.skins, def_map, defaults);
flexes.back()->def = defaults[def_map[flex->def]]; CopyList(hfields, other.hfields, def_map, defaults);
} CopyList(textures, other.textures, def_map, defaults);
for (const mjCMesh* mesh : other.meshes) { CopyList(materials, other.materials, def_map, defaults);
meshes.push_back(new mjCMesh(*mesh)); CopyList(pairs, other.pairs, def_map, defaults);
meshes.back()->model = this; CopyList(excludes, other.excludes, def_map, defaults);
meshes.back()->def = defaults[def_map[mesh->def]]; CopyList(tendons, other.tendons, def_map, defaults);
} CopyList(equalities, other.equalities, def_map, defaults);
for (const mjCSkin* skin : other.skins) { CopyList(actuators, other.actuators, def_map, defaults);
skins.push_back(new mjCSkin(*skin)); CopyList(sensors, other.sensors, def_map, defaults);
skins.back()->model = this; CopyList(numerics, other.numerics, def_map, defaults);
skins.back()->def = defaults[def_map[skin->def]]; CopyList(texts, other.texts, def_map, defaults);
} CopyList(tuples, other.tuples, def_map, defaults);
for (const mjCHField* hfield : other.hfields) { CopyList(keys, other.keys, def_map, defaults);
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]];
}
// plugins are global // plugins are global
plugins = other.plugins; plugins = other.plugins;
active_plugins = other.active_plugins; active_plugins = other.active_plugins;
// create global lists
MakeLists(bodies[0]);
// update defaults for the copied objects // update defaults for the copied objects
for (int i = 1; i < other.bodies.size(); i++) { for (int i = 1; i < other.bodies.size(); i++) {
bodies[i]->def = defaults[def_map[other.bodies[i]->def]]; bodies[i]->def = defaults[def_map[other.bodies[i]->def]];
@@ -296,7 +263,7 @@ mjCModel& mjCModel::operator+=(const mjCModel& other) {
} }
// cast children to mjCBase // cast children to mjCBase
CreateObjectLists();
// restore to the same state as other // restore to the same state as other
if (!compiled) { if (!compiled) {
@@ -670,28 +637,28 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) {
//------------------------ API FOR ACCESS TO PRIVATE VARIABLES ------------------------------------- //------------------------ API FOR ACCESS TO PRIVATE VARIABLES -------------------------------------
// compiled flag // compiled flag
bool mjCModel::IsCompiled(void) { bool mjCModel::IsCompiled(void) const {
return compiled; return compiled;
} }
// get reference of error object // get reference of error object
const mjCError& mjCModel::GetError(void) { const mjCError& mjCModel::GetError(void) const {
return errInfo; return errInfo;
} }
// pointer to world body // pointer to world body
mjCBody* mjCModel::GetWorld(void) { mjCBody* mjCModel::GetWorld(void) const {
return bodies[0]; return bodies[0];
} }
// find default class name in array // 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++) { for (int i=0; i<(int)defaults.size(); i++) {
if (defaults[i]->name==name) { if (defaults[i]->name==name) {
return defaults[i]; return defaults[i];
@@ -754,7 +721,7 @@ static T* findobject(std::string_view name, const vector<T*>& list, const mjKeyM
} }
// find object in global lists given string type and name // 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]) { if (!object_lists[type]) {
return nullptr; return nullptr;
} }
@@ -764,7 +731,7 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) {
// find body by 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) { if (body->name == name) {
return body; return body;
} }
@@ -782,7 +749,7 @@ mjCBody* mjCModel::FindBody(mjCBody* body, std::string name) {
// find frame by 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) { for (auto frame : body->frames) {
if (frame->name == name) { if (frame->name == name) {
return frame; return frame;
@@ -802,7 +769,7 @@ mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) {
// detect null pose // 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; bool result = true;
// check position if given // check position if given
@@ -2913,6 +2880,21 @@ static void processlist(mjListKeyMap& ids, vector<T*>& 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 // error handler for low-level engine
static thread_local std::jmp_buf error_jmp_buf; static thread_local std::jmp_buf error_jmp_buf;
static thread_local char errortext[500] = ""; static thread_local char errortext[500] = "";
@@ -3046,14 +3028,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
CheckEmptyNames(); CheckEmptyNames();
// set object ids, check for repeated names // set object ids, check for repeated names
for (int i = 0; i < mjNOBJECT; i++) { ProcessLists();
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);
// delete visual assets // delete visual assets
if (discardvisual) { if (discardvisual) {
+23 -9
View File
@@ -34,6 +34,11 @@ typedef std::map<std::string, int, std::less<> > mjKeyMap;
typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap; typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap;
class mjCModel_ { class mjCModel_ {
public:
// attach namespaces
std::string prefix;
std::string suffix;
protected: protected:
bool compiled; // already compiled flag bool compiled; // already compiled flag
@@ -185,6 +190,12 @@ class mjCModel : public mjCModel_, private mjSpec {
mjCKey* AddKey(); mjCKey* AddKey();
mjCPlugin* AddPlugin(); mjCPlugin* AddPlugin();
// copy vector of elements to this model
template <class T> void CopyList(std::vector<T*>& dest,
const std::vector<T*>& source,
std::map<mjCDef*, int>& def_map,
const std::vector<mjCDef*>& defaults);
// delete elements marked as discard=true // delete elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements, template <class T> void Delete(std::vector<T*>& elements,
const std::vector<bool>& discard); const std::vector<bool>& discard);
@@ -197,15 +208,15 @@ class mjCModel : public mjCModel_, private mjSpec {
mjCBase* GetObject(mjtObj type, int id); // pointer to specified object mjCBase* GetObject(mjtObj type, int id); // pointer to specified object
// API for access to other variables // API for access to other variables
bool IsCompiled(); // is model already compiled bool IsCompiled() const; // is model already compiled
const mjCError& GetError(void); // get reference of error object const mjCError& GetError(void) const; // get reference of error object
mjCBody* GetWorld(); // pointer to world body mjCBody* GetWorld() const; // pointer to world body
mjCDef* FindDef(std::string name); // find default class name mjCDef* FindDef(std::string name) const; // find default class name
mjCDef* AddDef(std::string name, int parentid); // add default class to array 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 mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name
mjCBody* FindBody(mjCBody* body, std::string name); // find body given name mjCBody* FindBody(mjCBody* body, std::string name) const; // find body given name
mjCFrame* FindFrame(mjCBody* body, std::string name); // find frame given name mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose
// accessors // accessors
std::string get_meshdir(void) const { return meshdir_; } std::string get_meshdir(void) const { return meshdir_; }
@@ -289,6 +300,9 @@ class mjCModel : public mjCModel_, private mjSpec {
// create mjCBase lists from children lists // create mjCBase lists from children lists
void CreateObjectLists(); void CreateObjectLists();
// populate objects ids
void ProcessLists();
mjListKeyMap ids; // map from object names to ids mjListKeyMap ids; // map from object names to ids
mjCError errInfo; // last error info mjCError errInfo; // last error info
bool plugin_owner; // this class allocated the plugins bool plugin_owner; // this class allocated the plugins
+267 -105
View File
@@ -793,15 +793,45 @@ mjCBody::~mjCBody() {
// apply prefix and suffix, propagate to children // apply prefix and suffix, propagate to children
void mjCBody::SetNameSpace() { void mjCBody::NameSpace(const mjCModel* m) {
if (!name.empty()) { if (!name.empty()) {
name = prefix + name + suffix; 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) { for (auto& body : bodies) {
body->prefix = prefix; body->prefix = m->prefix;
body->suffix = suffix; body->suffix = m->suffix;
body->SetNameSpace(); body->NameSpace(m);
} }
} }
@@ -1368,8 +1398,10 @@ mjCFrame& mjCFrame::operator=(const mjCFrame& other) {
// attach body to frame // attach body to frame
mjCFrame& mjCFrame::operator+=(const mjCBody& other) { mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
mjCBody* subtree = new mjCBody(other, model); mjCBody* subtree = new mjCBody(other, model);
other.model->prefix = subtree->prefix;
other.model->suffix = subtree->suffix;
subtree->SetFrame(this); subtree->SetFrame(this);
subtree->SetNameSpace(); subtree->NameSpace(other.model);
// add to tree // add to tree
*body += *subtree; *body += *subtree;
@@ -3777,8 +3809,8 @@ mjCPair& mjCPair::operator=(const mjCPair& other) {
this->spec = other.spec; this->spec = other.spec;
*static_cast<mjCPair_*>(this) = static_cast<const mjCPair_&>(other); *static_cast<mjCPair_*>(this) = static_cast<const mjCPair_&>(other);
*static_cast<mjmPair*>(this) = static_cast<const mjmPair&>(other); *static_cast<mjmPair*>(this) = static_cast<const mjmPair&>(other);
this->geom1 = other.geom1; this->geom1 = nullptr;
this->geom2 = other.geom2; this->geom2 = nullptr;
} }
PointToLocal(); PointToLocal();
return *this; 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() { void mjCPair::CopyFromSpec() {
*static_cast<mjmPair*>(this) = spec; *static_cast<mjmPair*>(this) = spec;
geomname1_ = spec_geomname1_; geomname1_ = spec_geomname1_;
@@ -3807,31 +3849,19 @@ void mjCPair::CopyFromSpec() {
// compiler void mjCPair::ResolveReferences(const mjCModel* m) {
void mjCPair::Compile(void) {
CopyFromSpec();
// check condim
if (condim!=1 && condim!=3 && condim!=4 && condim!=6) {
throw mjCError(this, "invalid condim in collision %d", "", id);
}
// find geom 1 // find geom 1
geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1_); geom1 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname1_);
if (!geom1) { if (!geom1) {
throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id); throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id);
} }
// find geom 2 // find geom 2
geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2_); geom2 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname2_);
if (!geom2) { if (!geom2) {
throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id); throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id);
} }
// mark geoms as not visual
geom1->SetNotVisual();
geom2->SetNotVisual();
// swap if body1 > body2 // swap if body1 > body2
if (geom1->body->id > geom2->body->id) { if (geom1->body->id > geom2->body->id) {
string nametmp = geomname1_; string nametmp = geomname1_;
@@ -3845,6 +3875,25 @@ void mjCPair::Compile(void) {
// get geom ids and body signature // get geom ids and body signature
signature = ((geom1->body->id)<<16) + geom2->body->id; 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 // set undefined margin: max
if (!mjuu_defined(margin)) { 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() { void mjCBodyPair::CopyFromSpec() {
*static_cast<mjmExclude*>(this) = spec; *static_cast<mjmExclude*>(this) = spec;
bodyname1_ = spec_bodyname1_; bodyname1_ = spec_bodyname1_;
@@ -3999,18 +4058,15 @@ void mjCBodyPair::CopyFromSpec() {
// compiler void mjCBodyPair::ResolveReferences(const mjCModel* m) {
void mjCBodyPair::Compile(void) {
CopyFromSpec();
// find body 1 // find body 1
mjCBody* pb1 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname1_); mjCBody* pb1 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname1_);
if (!pb1) { 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 // find body 2
mjCBody* pb2 = (mjCBody*)model->FindObject(mjOBJ_BODY, bodyname2_); mjCBody* pb2 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname2_);
if (!pb2) { 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);
} }
@@ -4034,6 +4090,16 @@ void mjCBodyPair::Compile(void) {
// compiler
void mjCBodyPair::Compile(void) {
CopyFromSpec();
// find bodies
ResolveReferences(model);
}
//------------------ class mjCEquality implementation ---------------------------------------------- //------------------ class mjCEquality implementation ----------------------------------------------
// initialize default constraint // 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() { void mjCEquality::CopyFromSpec() {
*static_cast<mjmEquality*>(this) = spec; *static_cast<mjmEquality*>(this) = spec;
name1_ = spec_name1_; name1_ = spec_name1_;
@@ -4100,10 +4177,8 @@ void mjCEquality::CopyFromSpec() {
} }
// compiler
void mjCEquality::Compile(void) {
CopyFromSpec();
void mjCEquality::ResolveReferences(const mjCModel* m) {
mjtObj objtype; mjtObj objtype;
mjCBase *px1, *px2; mjCBase *px1, *px2;
mjtJoint jt1, jt2; mjtJoint jt1, jt2;
@@ -4122,7 +4197,7 @@ void mjCEquality::Compile(void) {
} }
// find object 1, get id // find object 1, get id
px1 = model->FindObject(objtype, name1_); px1 = m->FindObject(objtype, name1_);
if (!px1) { 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);
} }
@@ -4130,7 +4205,7 @@ void mjCEquality::Compile(void) {
// find object 2, get id // find object 2, get id
if (!name2_.empty()) { if (!name2_.empty()) {
px2 = model->FindObject(objtype, name2_); px2 = m->FindObject(objtype, name2_);
if (!px2) { 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);
} }
@@ -4153,11 +4228,6 @@ void mjCEquality::Compile(void) {
obj2id = 0; 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 // make sure the two objects are different
if (obj1id==obj2id) { 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);
@@ -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 ------------------------------------------------ //------------------ class mjCTendon implementation ------------------------------------------------
// constructor // 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; i<path.size(); i++) {
path[i]->NameSpace(model);
}
}
void mjCTendon::CopyFromSpec() { void mjCTendon::CopyFromSpec() {
*static_cast<mjmTendon*>(this) = spec; *static_cast<mjmTendon*>(this) = spec;
material_ = spec_material_; material_ = spec_material_;
@@ -4361,6 +4457,14 @@ mjCWrap* mjCTendon::GetWrap(int id) {
void mjCTendon::ResolveReferences(const mjCModel* m) {
for (int i=0; i<path.size(); i++) {
path[i]->ResolveReferences(m);
}
}
// compiler // compiler
void mjCTendon::Compile(void) { void mjCTendon::Compile(void) {
CopyFromSpec(); CopyFromSpec();
@@ -4395,9 +4499,7 @@ void mjCTendon::Compile(void) {
} }
// compile objects in path // compile objects in path
for (int i=0; i<sz; i++) { ResolveReferences(model);
path[i]->Compile();
}
// check path // check path
for (int i=0; i<sz; i++) { for (int i=0; i<sz; i++) {
@@ -4536,15 +4638,21 @@ void mjCWrap::PointToLocal() {
// compiler void mjCWrap::NameSpace(const mjCModel* m) {
void mjCWrap::Compile(void) { name = m->prefix + name + m->suffix;
sidesite = m->prefix + sidesite + m->suffix;
}
void mjCWrap::ResolveReferences(const mjCModel* m) {
mjCBase *pside; mjCBase *pside;
// handle wrap object types // handle wrap object types
switch (type) { switch (type) {
case mjWRAP_JOINT: // joint case mjWRAP_JOINT: // joint
// find joint by name // find joint by name
obj = model->FindObject(mjOBJ_JOINT, name); obj = m->FindObject(mjOBJ_JOINT, name);
if (!obj) { if (!obj) {
throw mjCError(this, throw mjCError(this,
"joint '%s' not found in tendon %d, wrap %d", "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) case mjWRAP_SPHERE: // geom (cylinder type set here)
// find geom by name // find geom by name
obj = model->FindObject(mjOBJ_GEOM, name); obj = m->FindObject(mjOBJ_GEOM, name);
if (!obj) { if (!obj) {
throw mjCError(this, throw mjCError(this,
"geom '%s' not found in tendon %d, wrap %d", "geom '%s' not found in tendon %d, wrap %d",
@@ -4574,7 +4682,7 @@ void mjCWrap::Compile(void) {
// process side site // process side site
if (!sidesite.empty()) { if (!sidesite.empty()) {
// find site by name // find site by name
pside = model->FindObject(mjOBJ_SITE, sidesite); pside = m->FindObject(mjOBJ_SITE, sidesite);
if (!pside) { if (!pside) {
throw mjCError(this, throw mjCError(this,
"side site '%s' not found in tendon %d, wrap %d", "side site '%s' not found in tendon %d, wrap %d",
@@ -4598,7 +4706,7 @@ void mjCWrap::Compile(void) {
case mjWRAP_SITE: // site case mjWRAP_SITE: // site
// find site by name // find site by name
obj = model->FindObject(mjOBJ_SITE, name); obj = m->FindObject(mjOBJ_SITE, name);
if (!obj) { if (!obj) {
throw mjCError(this, "site '%s' not found in wrap %d", name.c_str(), id); 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); mjm_defaultActuator(spec);
// clear private variables // clear private variables
ptarget = nullptr;
spec_target_.clear(); spec_target_.clear();
spec_slidersite_.clear(); spec_slidersite_.clear();
spec_refsite_.clear(); spec_refsite_.clear();
@@ -4653,6 +4762,7 @@ mjCActuator& mjCActuator::operator=(const mjCActuator& other) {
this->spec = other.spec; this->spec = other.spec;
*static_cast<mjCActuator_*>(this) = static_cast<const mjCActuator_&>(other); *static_cast<mjCActuator_*>(this) = static_cast<const mjCActuator_&>(other);
*static_cast<mjmActuator*>(this) = static_cast<const mjmActuator&>(other); *static_cast<mjmActuator*>(this) = static_cast<const mjmActuator&>(other);
ptarget = nullptr;
} }
PointToLocal(); PointToLocal();
return *this; 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() { void mjCActuator::CopyFromSpec() {
*static_cast<mjmActuator*>(this) = spec; *static_cast<mjmActuator*>(this) = spec;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
@@ -4699,31 +4820,13 @@ void mjCActuator::CopyFromSpec() {
// compiler void mjCActuator::ResolveReferences(const mjCModel* m) {
void mjCActuator::Compile(void) {
CopyFromSpec();
mjCJoint* pjnt; 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) { switch (trntype) {
case mjTRN_JOINT: case mjTRN_JOINT:
case mjTRN_JOINTINPARENT: case mjTRN_JOINTINPARENT:
// get joint // get joint
ptarget = model->FindObject(mjOBJ_JOINT, target_); ptarget = m->FindObject(mjOBJ_JOINT, target_);
if (!ptarget) { if (!ptarget) {
throw mjCError(this, throw mjCError(this,
"unknown transmission target '%s' for actuator id = %d", target_.c_str(), id); "unknown transmission target '%s' for actuator id = %d", target_.c_str(), id);
@@ -4743,7 +4846,7 @@ void mjCActuator::Compile(void) {
if (slidersite_.empty()) { if (slidersite_.empty()) {
throw mjCError(this, "missing base site for slider-crank '%s' (id = %d)", name.c_str(), id); 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) { if (!ptarget) {
throw mjCError(this, "base site '%s' not found for actuator %d", slidersite_.c_str(), id); 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 // proceed with regular target
ptarget = model->FindObject(mjOBJ_SITE, target_); ptarget = m->FindObject(mjOBJ_SITE, target_);
break; break;
case mjTRN_TENDON: case mjTRN_TENDON:
// get tendon // get tendon
ptarget = model->FindObject(mjOBJ_TENDON, target_); ptarget = m->FindObject(mjOBJ_TENDON, target_);
break; break;
case mjTRN_SITE: case mjTRN_SITE:
// get refsite, copy into trnid[1] // get refsite, copy into trnid[1]
if (!refsite_.empty()) { if (!refsite_.empty()) {
ptarget = model->FindObject(mjOBJ_SITE, refsite_); ptarget = m->FindObject(mjOBJ_SITE, refsite_);
if (!ptarget) { if (!ptarget) {
throw mjCError(this, "reference site '%s' not found for actuator %d", refsite_.c_str(), id); 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 // proceed with regular site target
ptarget = model->FindObject(mjOBJ_SITE, target_); ptarget = m->FindObject(mjOBJ_SITE, target_);
break; break;
case mjTRN_BODY: case mjTRN_BODY:
// get body // get body
ptarget = model->FindObject(mjOBJ_BODY, target_); ptarget = m->FindObject(mjOBJ_BODY, target_);
break; break;
default: default:
@@ -4793,6 +4896,29 @@ void mjCActuator::Compile(void) {
} else { } else {
trnid[0] = ptarget->id; 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 // handle inheritrange
if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE && if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE &&
@@ -4812,7 +4938,7 @@ void mjCActuator::Compile(void) {
const double* target_range; const double* target_range;
if (trntype == mjTRN_JOINT) { if (trntype == mjTRN_JOINT) {
pjnt = (mjCJoint*) ptarget; mjCJoint* pjnt = (mjCJoint*) ptarget;
if (pjnt->spec.type != mjJNT_HINGE && pjnt->spec.type != mjJNT_SLIDE) { if (pjnt->spec.type != mjJNT_HINGE && pjnt->spec.type != mjJNT_SLIDE) {
throw mjCError(this, "inheritrange can only be used with hinge and slide joints, " throw mjCError(this, "inheritrange can only be used with hinge and slide joints, "
"actuator '%s' (id = %d)", name.c_str(), id); "actuator '%s' (id = %d)", name.c_str(), id);
@@ -4950,6 +5076,7 @@ mjCSensor::mjCSensor(mjCModel* _model) {
spec_refname_.clear(); spec_refname_.clear();
spec_userdata_.clear(); spec_userdata_.clear();
obj = nullptr; obj = nullptr;
ref = nullptr;
refid = -1; refid = -1;
// in case this sensor is not compiled // in case this sensor is not compiled
@@ -4972,6 +5099,8 @@ mjCSensor& mjCSensor::operator=(const mjCSensor& other) {
this->spec = other.spec; this->spec = other.spec;
*static_cast<mjCSensor_*>(this) = static_cast<const mjCSensor_&>(other); *static_cast<mjCSensor_*>(this) = static_cast<const mjCSensor_&>(other);
*static_cast<mjmSensor*>(this) = static_cast<const mjmSensor&>(other); *static_cast<mjmSensor*>(this) = static_cast<const mjmSensor&>(other);
obj = nullptr;
ref = nullptr;
} }
PointToLocal(); PointToLocal();
return *this; 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() { void mjCSensor::CopyFromSpec() {
*static_cast<mjmSensor*>(this) = spec; *static_cast<mjmSensor*>(this) = spec;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
@@ -5009,27 +5148,7 @@ void mjCSensor::CopyFromSpec() {
// compiler void mjCSensor::ResolveReferences(const mjCModel* m) {
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);
}
// get objid from objtype and objname // get objid from objtype and objname
if (objtype!=mjOBJ_UNKNOWN) { if (objtype!=mjOBJ_UNKNOWN) {
// check for missing object name // check for missing object name
@@ -5040,7 +5159,7 @@ void mjCSensor::Compile(void) {
} }
// find name // find name
obj = model->FindObject(objtype, objname_); obj = m->FindObject(objtype, objname_);
if (!obj) { if (!obj) {
throw mjCError(this, throw mjCError(this,
"unrecognized name of sensorized object in sensor '%s' (id = %d)", "unrecognized name of sensorized object in sensor '%s' (id = %d)",
@@ -5067,8 +5186,8 @@ void mjCSensor::Compile(void) {
} }
// find name // find name
mjCBase* pref = model->FindObject(reftype, refname_); ref = m->FindObject(reftype, refname_);
if (!pref) { if (!ref) {
throw mjCError(this, throw mjCError(this,
"unrecognized name of reference frame object in sensor '%s' (id = %d)", "unrecognized name of reference frame object in sensor '%s' (id = %d)",
name.c_str(), id); name.c_str(), id);
@@ -5083,8 +5202,35 @@ void mjCSensor::Compile(void) {
} }
// get sensorized object id // 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 // process according to sensor type
switch (type) { switch (type) {
@@ -5126,7 +5272,7 @@ void mjCSensor::Compile(void) {
// check for camera resolution for camera projection sensor // check for camera resolution for camera projection sensor
if (type==mjSENS_CAMPROJECTION) { if (type==mjSENS_CAMPROJECTION) {
mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname_); mjCCamera* camref = (mjCCamera*)ref;
if (!camref->resolution[0] || !camref->resolution[1]) { if (!camref->resolution[0] || !camref->resolution[1]) {
throw mjCError(this, throw mjCError(this,
"camera projection sensor requires camera resolution '%s' (id = %d)", "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; i<spec_objname_.size(); i++) {
spec_objname_[i] = m->prefix + spec_objname_[i] + m->suffix;
}
}
void mjCTuple::CopyFromSpec() { void mjCTuple::CopyFromSpec() {
*static_cast<mjmTuple*>(this) = spec; *static_cast<mjmTuple*>(this) = spec;
objtype_ = spec_objtype_; objtype_ = spec_objtype_;
@@ -5644,10 +5801,7 @@ mjCTuple::~mjCTuple() {
// compiler void mjCTuple::ResolveReferences(const mjCModel* m) {
void mjCTuple::Compile(void) {
CopyFromSpec();
// check for empty tuple // check for empty tuple
if (objtype_.empty()) { if (objtype_.empty()) {
throw mjCError(this, "tuple '%s' (id = %d) is empty", name.c_str(), id); 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 // find objects, fill in ids
for (int i=0; i<objtype_.size(); i++) { for (int i=0; i<objtype_.size(); i++) {
// find object by type and name // find object by type and name
mjCBase* res = model->FindObject(objtype_[i], objname_[i]); mjCBase* res = m->FindObject(objtype_[i], objname_[i]);
if (!res) { if (!res) {
throw mjCError(this, "unrecognized object '%s' in tuple %d", objname_[i].c_str(), id); 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 --------------------------------------------------- //------------------ class mjCKey implementation ---------------------------------------------------
// constructor // constructor
+33 -4
View File
@@ -198,6 +198,12 @@ class mjCBase : public mjCBase_ {
// Copy spec into private attributes // Copy spec into private attributes
virtual void CopyFromSpec() {} 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 // Copy assignment
mjCBase& operator=(const mjCBase& other); 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); mjCBase* FindObject(mjtObj type, std::string name, bool recursive = true);
// Propagate suffix and prefix to the whole tree // Propagate suffix and prefix to the whole tree
void SetNameSpace(); void NameSpace(const mjCModel* m);
// set explicitinertial to true // set explicitinertial to true
void MakeInertialExplicit(); void MakeInertialExplicit();
@@ -682,6 +688,8 @@ class mjCFlex: public mjCFlex_, private mjmFlex {
void CopyFromSpec(void); void CopyFromSpec(void);
void PointToLocal(void); void PointToLocal(void);
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
// used by mjXWriter and mjCModel // used by mjXWriter and mjCModel
const std::string& get_material() { return material_; } 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& operator=(const mjCSkin& other); // copy assignment
~mjCSkin(); // destructor ~mjCSkin(); // destructor
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
void Compile(const mjVFS* vfs); // compiler void Compile(const mjVFS* vfs); // compiler
void LoadSKN(mjResource* resource); // load skin in SKN BIN format void LoadSKN(mjResource* resource); // load skin in SKN BIN format
}; };
@@ -1115,6 +1125,8 @@ class mjCPair : public mjCPair_, private mjmPair {
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
std::string get_geomname1() { return geomname1_; } std::string get_geomname1() { return geomname1_; }
std::string get_geomname2() { return geomname2_; } std::string get_geomname2() { return geomname2_; }
@@ -1162,6 +1174,8 @@ class mjCBodyPair : public mjCBodyPair_, private mjmExclude {
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
std::string get_bodyname1() const { return bodyname1_; } std::string get_bodyname1() const { return bodyname1_; }
std::string get_bodyname2() const { return bodyname2_; } std::string get_bodyname2() const { return bodyname2_; }
@@ -1207,6 +1221,8 @@ class mjCEquality : public mjCEquality_, private mjmEquality {
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
private: private:
mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor
@@ -1264,6 +1280,8 @@ class mjCTendon : public mjCTendon_, private mjmTendon {
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
void SetModel(mjCModel* _model); void SetModel(mjCModel* _model);
bool is_limited() const; bool is_limited() const;
@@ -1299,6 +1317,9 @@ class mjCWrap : public mjCWrap_, private mjmWrap {
using mjCBase::info; using mjCBase::info;
void PointToLocal(); void PointToLocal();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
mjCBase* obj; // wrap object pointer mjCBase* obj; // wrap object pointer
private: private:
@@ -1306,8 +1327,6 @@ class mjCWrap : public mjCWrap_, private mjmWrap {
mjCWrap(const mjCWrap& other); // copy constructor mjCWrap(const mjCWrap& other); // copy constructor
mjCWrap& operator=(const mjCWrap& other); // copy assignment mjCWrap& operator=(const mjCWrap& other); // copy assignment
void Compile(void); // compiler
mjCTendon* tendon; // tendon owning this wrap mjCTendon* tendon; // tendon owning this wrap
}; };
@@ -1393,6 +1412,10 @@ class mjCActuator : public mjCActuator_, private mjmActuator {
void Compile(void); // compiler void Compile(void); // compiler
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); 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 { class mjCSensor_ : public mjCBase {
protected: protected:
int refid; // id of reference frame int refid; // id of reference frame
mjCBase* obj; // sensorized object
// variable-size data // variable-size data
std::string plugin_name; std::string plugin_name;
@@ -1440,6 +1462,11 @@ class mjCSensor : public mjCSensor_, private mjmSensor {
void Compile(void); // compiler void Compile(void); // compiler
void CopyFromSpec(); void CopyFromSpec();
void PointToLocal(); 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 PointToLocal();
void CopyFromSpec(); void CopyFromSpec();
void ResolveReferences(const mjCModel* m);
void NameSpace(const mjCModel* m);
private: private:
mjCTuple(mjCModel*); // constructor mjCTuple(mjCModel*); // constructor
+32 -6
View File
@@ -215,13 +215,27 @@ TEST_F(MujocoTest, Attach) {
static constexpr char xml_child[] = R"( static constexpr char xml_child[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
<body name="cylinder"> <body name="body">
<joint type="hinge"/> <joint type="hinge" name="hinge"/>
<geom type="cylinder" size=".1 1 0"/> <geom type="cylinder" size=".1 1 0"/>
<body name="named"/> <body name="named"/>
<body/> <body/>
</body> </body>
<body name="discard"/>
</worldbody> </worldbody>
<sensor>
<framepos name="keep" objtype="body" objname="body"/>
<framepos name="discard" objtype="body" objname="discard"/>
</sensor>
<tendon>
<fixed name="fixed">
<joint joint="hinge" coef="2"/>
</fixed>
</tendon>
<actuator>
<position joint="hinge"/>
<position tendon="fixed"/>
</actuator>
</mujoco>)"; </mujoco>)";
static constexpr char xml_result[] = R"( static constexpr char xml_result[] = R"(
@@ -231,8 +245,8 @@ TEST_F(MujocoTest, Attach) {
<freejoint/> <freejoint/>
<geom size=".1"/> <geom size=".1"/>
<frame name="frame" pos=".1 0 0" euler="0 90 0"> <frame name="frame" pos=".1 0 0" euler="0 90 0">
<body name="attached-cylinder-1"> <body name="attached-body-1">
<joint type="hinge"/> <joint type="hinge" name="attached-hinge-1"/>
<geom type="cylinder" size=".1 1 0"/> <geom type="cylinder" size=".1 1 0"/>
<body name="attached-named-1"/> <body name="attached-named-1"/>
<body/> <body/>
@@ -240,6 +254,18 @@ TEST_F(MujocoTest, Attach) {
</frame> </frame>
</body> </body>
</worldbody> </worldbody>
<sensor>
<framepos name="attached-keep-1" objtype="body" objname="attached-body-1"/>
</sensor>
<tendon>
<fixed name="attached-fixed-1">
<joint joint="attached-hinge-1" coef="2"/>
</fixed>
</tendon>
<actuator>
<position joint="attached-hinge-1"/>
<position tendon="attached-fixed-1"/>
</actuator>
</mujoco>)"; </mujoco>)";
// model with one free sphere and a frame // model with one free sphere and a frame
@@ -255,7 +281,7 @@ TEST_F(MujocoTest, Attach) {
EXPECT_THAT(child, NotNull()) << er.data(); EXPECT_THAT(child, NotNull()) << er.data();
// get subtree // get subtree
mjmBody* body = mjm_findBody(child, "cylinder"); mjmBody* body = mjm_findBody(child, "body");
EXPECT_THAT(body, NotNull()); EXPECT_THAT(body, NotNull());
// attach child to parent frame // attach child to parent frame
@@ -267,7 +293,7 @@ TEST_F(MujocoTest, Attach) {
EXPECT_THAT(m_attached, NotNull()); EXPECT_THAT(m_attached, NotNull());
// check full name stored in mjModel // 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 // check body 2 is attached to body 1
EXPECT_THAT(m_attached->body_parentid[2], 1); EXPECT_THAT(m_attached->body_parentid[2], 1);