chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Do not attach elements not referenced in the subtree.

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+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() {
*static_cast<mjmSkin*>(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; 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
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; 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;
}
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<mjmFlex*>(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++) {
src/user/user_model.cc
+75 -100
View File
@@ -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 <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) {
if (this != &other) {
// create global lists
MakeLists(bodies[0]);
CreateObjectLists();
ProcessLists();
// copy all elements not in the tree
std::map<mjCDef*, int> 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<T*>& 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<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
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) {
src/user/user_model.h
+23 -9
View File
@@ -34,6 +34,11 @@ typedef std::map<std::string, int, std::less<> > mjKeyMap;
typedef std::array<mjKeyMap, mjNOBJECT> 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 <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
template <class T> void Delete(std::vector<T*>& elements,
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
// 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
src/user/user_objects.cc
+267 -105
View File
@@ -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<mjCPair_*>(this) = static_cast<const mjCPair_&>(other);
*static_cast<mjmPair*>(this) = static_cast<const mjmPair&>(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<mjmPair*>(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<mjmExclude*>(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<mjmEquality*>(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; i<path.size(); i++) {
path[i]->NameSpace(model);
}
}
void mjCTendon::CopyFromSpec() {
*static_cast<mjmTendon*>(this) = spec;
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
void mjCTendon::Compile(void) {
CopyFromSpec();
@@ -4395,9 +4499,7 @@ void mjCTendon::Compile(void) {
}
// compile objects in path
for (int i=0; i<sz; i++) {
path[i]->Compile();
}
ResolveReferences(model);
// check path
for (int i=0; i<sz; i++) {
@@ -4536,15 +4638,21 @@ void mjCWrap::PointToLocal() {
// compiler
void mjCWrap::Compile(void) {
void mjCWrap::NameSpace(const mjCModel* m) {
name = m->prefix + 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<mjCActuator_*>(this) = static_cast<const mjCActuator_&>(other);
*static_cast<mjmActuator*>(this) = static_cast<const mjmActuator&>(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<mjmActuator*>(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<mjCSensor_*>(this) = static_cast<const mjCSensor_&>(other);
*static_cast<mjmSensor*>(this) = static_cast<const mjmSensor&>(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<mjmSensor*>(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; i<spec_objname_.size(); i++) {
spec_objname_[i] = m->prefix + spec_objname_[i] + m->suffix;
}
}
void mjCTuple::CopyFromSpec() {
*static_cast<mjmTuple*>(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; i<objtype_.size(); i++) {
// find object by type and name
mjCBase* res = model->FindObject(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
src/user/user_objects.h
+33 -4
View File
@@ -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
test/user/user_api_test.cc
+32 -6
View File
@@ -215,13 +215,27 @@ TEST_F(MujocoTest, Attach) {
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="cylinder">
<joint type="hinge"/>
<body name="body">
<joint type="hinge" name="hinge"/>
<geom type="cylinder" size=".1 1 0"/>
<body name="named"/>
<body/>
</body>
<body name="discard"/>
</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>)";
static constexpr char xml_result[] = R"(
@@ -231,8 +245,8 @@ TEST_F(MujocoTest, Attach) {
<freejoint/>
<geom size=".1"/>
<frame name="frame" pos=".1 0 0" euler="0 90 0">
<body name="attached-cylinder-1">
<joint type="hinge"/>
<body name="attached-body-1">
<joint type="hinge" name="attached-hinge-1"/>
<geom type="cylinder" size=".1 1 0"/>
<body name="attached-named-1"/>
<body/>
@@ -240,6 +254,18 @@ TEST_F(MujocoTest, Attach) {
</frame>
</body>
</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>)";
// 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);