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

Allow attaching a model to itself.

Browse Source 
PiperOrigin-RevId: 623806812
Change-Id: I9b0e36308be754e838790ead8ec717953e4b2350
This commit is contained in:
Alessio Quaglino
2024-04-11 06:27:44 -07:00
committed by Copybara-Service
parent 225619c964
commit 70f00b05a1
5 changed files with 224 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
-5
View File
@@ -254,11 +254,6 @@ mjCMesh& mjCMesh::operator=(const mjCMesh& other) {
} else {
this->graph_ = NULL;
}
if (other.plugin.instance) {
mjCPlugin* new_plugin = new mjCPlugin(*static_cast<mjCPlugin*>(other.plugin.instance));
plugin = new_plugin->spec;
model->plugins.push_back(new_plugin);
}
}
PointToLocal();
return *this;
src/user/user_model.cc
+77 -73
View File
@@ -190,95 +190,99 @@ void mjCModel::CopyList(std::vector<T*>& dest,
std::map<mjCDef*, int>& def_map,
const std::vector<mjCDef*>& defaults) {
// loop over the elements from the other model
for (T* element : source) {
int nsource = (int)source.size();
for (int i = 0; i < nsource; i++) {
T* candidate = new T(*source[i]);
try {
// try to find the referenced object in this model
element->NameSpace(element->model);
element->CopyFromSpec();
element->ResolveReferences(this);
candidate->NameSpace(source[i]->model);
candidate->CopyFromSpec();
candidate->ResolveReferences(this);
} catch (mjCError err) {
// if not present, skip the element
delete candidate;
continue;
}
// copy the element from the other model to this model
dest.push_back(new T(*element));
dest.push_back(candidate);
dest.back()->model = this;
dest.back()->def = defaults[def_map[element->def]];
dest.back()->def = defaults[def_map[candidate->def]];
dest.back()->id = -1;
}
if (!dest.empty()) {
processlist(ids, dest, dest[0]->elemtype);
}
}
mjCModel& mjCModel::operator+=(const mjCModel& other) {
if (this != &other) {
// create global lists
MakeLists(bodies[0]);
CreateObjectLists();
ProcessLists();
// 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;
}
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;
// 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]];
}
for (int i = 0; i < other.joints.size(); i++) {
joints[i]->def = defaults[def_map[other.joints[i]->def]];
}
for (int i = 0; i < other.geoms.size(); i++) {
geoms[i]->def = defaults[def_map[other.geoms[i]->def]];
}
for (int i = 0; i < other.sites.size(); i++) {
sites[i]->def = defaults[def_map[other.sites[i]->def]];
}
for (int i = 0; i < other.cameras.size(); i++) {
cameras[i]->def = defaults[def_map[other.cameras[i]->def]];
}
for (int i = 0; i < other.lights.size(); i++) {
lights[i]->def = defaults[def_map[other.lights[i]->def]];
}
// cast children to mjCBase
// restore to the same state as other
if (!compiled) {
mjCBody* world = bodies[0];
bodies.clear();
frames.clear();
joints.clear();
geoms.clear();
sites.clear();
cameras.clear();
lights.clear();
bodies.push_back(world);
}
// copy all elements not in the tree
std::map<mjCDef*, int> def_map;
int ndefaults = (int)other.defaults.size();
for (int i = 0; i < ndefaults; i++) {
defaults.push_back(new mjCDef(*other.defaults[i]));
def_map[other.defaults[i]] = i;
}
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;
// 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]];
}
for (int i = 0; i < other.joints.size(); i++) {
joints[i]->def = defaults[def_map[other.joints[i]->def]];
}
for (int i = 0; i < other.geoms.size(); i++) {
geoms[i]->def = defaults[def_map[other.geoms[i]->def]];
}
for (int i = 0; i < other.sites.size(); i++) {
sites[i]->def = defaults[def_map[other.sites[i]->def]];
}
for (int i = 0; i < other.cameras.size(); i++) {
cameras[i]->def = defaults[def_map[other.cameras[i]->def]];
}
for (int i = 0; i < other.lights.size(); i++) {
lights[i]->def = defaults[def_map[other.lights[i]->def]];
}
// restore to the same state as other
if (!compiled) {
mjCBody* world = bodies[0];
bodies.clear();
frames.clear();
joints.clear();
geoms.clear();
sites.clear();
cameras.clear();
lights.clear();
bodies.push_back(world);
}
PointToLocal();
return *this;
}
src/user/user_objects.cc
+26 -7
View File
@@ -815,15 +815,11 @@ void mjCBody::NameSpace(const mjCModel* m) {
}
for (auto& camera : cameras) {
if (!camera->name.empty()) {
camera->name = prefix + camera->name + suffix;
}
camera->NameSpace(m);
}
for (auto& light : lights) {
if (!light->name.empty()) {
light->name = prefix + light->name + suffix;
}
light->NameSpace(m);
}
for (auto& body : bodies) {
@@ -1408,6 +1404,10 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
// TODO: needs to attach only referencing elements
*model += *other.model;
// clear suffixes and return
other.model->suffix.clear();
other.model->prefix.clear();
return *this;
}
@@ -2501,6 +2501,15 @@ void mjCCamera::PointToLocal() {
void mjCCamera::NameSpace(const mjCModel* m) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
}
void mjCCamera::CopyFromSpec() {
*static_cast<mjsCamera*>(this) = spec;
userdata_ = spec_userdata_;
@@ -2648,6 +2657,15 @@ void mjCLight::PointToLocal() {
void mjCLight::NameSpace(const mjCModel* m) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
}
void mjCLight::CopyFromSpec() {
*static_cast<mjsLight*>(this) = spec;
targetbody_ = spec_targetbody_;
@@ -3990,6 +4008,7 @@ void mjCPair::Compile(void) {
mjCBodyPair::mjCBodyPair(mjCModel* _model) {
// set model pointer
model = _model;
elemtype = mjOBJ_EXCLUDE;
// set defaults
spec_bodyname1_.clear();
@@ -4329,7 +4348,7 @@ void mjCTendon::NameSpace(const mjCModel* m) {
name = m->prefix + name + m->suffix;
}
for (int i=0; i<path.size(); i++) {
path[i]->NameSpace(model);
path[i]->NameSpace(m);
}
}
src/user/user_objects.h
+2
View File
@@ -584,6 +584,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera {
void Compile(void); // compiler
void CopyFromSpec(void);
void PointToLocal(void);
void NameSpace(const mjCModel* m);
};
@@ -622,6 +623,7 @@ class mjCLight : public mjCLight_, private mjsLight {
void Compile(void); // compiler
void CopyFromSpec(void);
void PointToLocal(void);
void NameSpace(const mjCModel* m);
};