Allow attaching a model to itself.
PiperOrigin-RevId: 623806812 Change-Id: I9b0e36308be754e838790ead8ec717953e4b2350
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
225619c964
commit
70f00b05a1
@@ -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;
|
||||
|
||||
+77
-73
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
+119
-1
@@ -196,7 +196,125 @@ TEST_F(PluginTest, RecompileCompareCache) {
|
||||
}
|
||||
|
||||
// -------------------------------- test attach -------------------------------
|
||||
TEST_F(MujocoTest, Attach) {
|
||||
TEST_F(MujocoTest, AttachSame) {
|
||||
std::array<char, 1000> er;
|
||||
mjtNum tol = 0;
|
||||
std::string field = "";
|
||||
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="body">
|
||||
<joint type="hinge" name="hinge"/>
|
||||
<geom type="cylinder" size=".1 1 0"/>
|
||||
<light mode="targetbody" target="child"/>
|
||||
<body name="child"/>
|
||||
</body>
|
||||
<body name="ignore"/>
|
||||
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
|
||||
</worldbody>
|
||||
<sensor>
|
||||
<framepos name="sensor" objtype="body" objname="body"/>
|
||||
<framepos name="ignore" objtype="body" objname="ignore"/>
|
||||
</sensor>
|
||||
<tendon>
|
||||
<fixed name="fixed">
|
||||
<joint joint="hinge" coef="2"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
<actuator>
|
||||
<position name="hinge" joint="hinge"/>
|
||||
<position name="fixed" tendon="fixed"/>
|
||||
</actuator>
|
||||
<contact>
|
||||
<exclude body1="body" body2="child"/>
|
||||
</contact>
|
||||
</mujoco>)";
|
||||
|
||||
static constexpr char xml_result[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="body">
|
||||
<joint type="hinge" name="hinge"/>
|
||||
<geom type="cylinder" size=".1 1 0"/>
|
||||
<light mode="targetbody" target="child"/>
|
||||
<body name="child"/>
|
||||
</body>
|
||||
<body name="ignore"/>
|
||||
<frame name="frame" pos=".1 0 0" euler="0 90 0">
|
||||
<body name="attached-body-1">
|
||||
<joint type="hinge" name="attached-hinge-1"/>
|
||||
<geom type="cylinder" size=".1 1 0"/>
|
||||
<light mode="targetbody" target="attached-child-1"/>
|
||||
<body name="attached-child-1"/>
|
||||
</body>
|
||||
</frame>
|
||||
</worldbody>
|
||||
<sensor>
|
||||
<framepos name="sensor" objtype="body" objname="body"/>
|
||||
<framepos name="ignore" objtype="body" objname="ignore"/>
|
||||
<framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
|
||||
</sensor>
|
||||
<tendon>
|
||||
<fixed name="fixed">
|
||||
<joint joint="hinge" coef="2"/>
|
||||
</fixed>
|
||||
<fixed name="attached-fixed-1">
|
||||
<joint joint="attached-hinge-1" coef="2"/>
|
||||
</fixed>
|
||||
</tendon>
|
||||
<actuator>
|
||||
<position name="hinge" joint="hinge"/>
|
||||
<position name="fixed" tendon="fixed"/>
|
||||
<position name="attached-hinge-1" joint="attached-hinge-1"/>
|
||||
<position name="attached-fixed-1" tendon="attached-fixed-1"/>
|
||||
</actuator>
|
||||
<contact>
|
||||
<exclude body1="body" body2="child"/>
|
||||
<exclude body1="attached-body-1" body2="attached-child-1"/>
|
||||
</contact>
|
||||
</mujoco>)";
|
||||
|
||||
// create parent
|
||||
mjSpec* parent = ParseSpecFromString(xml, er.data(), er.size());
|
||||
EXPECT_THAT(parent, NotNull()) << er.data();
|
||||
|
||||
// get frame
|
||||
mjsFrame* frame = mjs_findFrame(parent, "frame");
|
||||
EXPECT_THAT(frame, NotNull());
|
||||
|
||||
// get subtree
|
||||
mjsBody* body = mjs_findBody(parent, "body");
|
||||
EXPECT_THAT(body, NotNull());
|
||||
|
||||
// attach child to parent frame
|
||||
EXPECT_THAT(
|
||||
mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0);
|
||||
|
||||
// compile new model
|
||||
mjModel* m_attached = mjs_compile(parent, 0);
|
||||
EXPECT_THAT(m_attached, NotNull());
|
||||
|
||||
// check full name stored in mjModel
|
||||
EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 4), "attached-body-1");
|
||||
|
||||
// check body 3 is attached to the world
|
||||
EXPECT_THAT(m_attached->body_parentid[3], 0);
|
||||
|
||||
// compare with expected XML
|
||||
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
|
||||
EXPECT_THAT(m_expected, NotNull()) << er.data();
|
||||
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
|
||||
<< "Expected and attached models are different!\n"
|
||||
<< "Different field: " << field << '\n';;
|
||||
|
||||
// destroy everything
|
||||
mjs_deleteSpec(parent);
|
||||
mj_deleteModel(m_attached);
|
||||
mj_deleteModel(m_expected);
|
||||
}
|
||||
|
||||
TEST_F(MujocoTest, AttachDifferent) {
|
||||
std::array<char, 1000> er;
|
||||
mjtNum tol = 0;
|
||||
std::string field = "";
|
||||
|
||||
Reference in New Issue
Block a user