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
+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