diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 71963d33..4e867448 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -792,12 +792,14 @@ has any effect. The settings here are global and apply to the entire model. :at:`fusestatic`: :at-val:`[false, true], "false" for MJCF, "true" for URDF` This attribute controls a compiler optimization feature where static bodies are fused with their parent, and any - elements defined in those bodies are reassigned to the parent. This feature can only be used in models which do not - have elements capable of named references inside the kinematic tree - namely skins, contact pairs, excludes, tendons, - actuators, sensors, tuples, cameras, lights. If a model has any these elements, fusestatic does nothing even if - enabled. This optimization is particularly useful when importing URDF models which often have many dummy bodies, but - can also be used to optimize MJCF models. After optimization, the new model has identical kinematics and dynamics as - the original but is faster to simulate. + elements defined in those bodies are reassigned to the parent. Static bodies are fused with their parent unless + + - They are referenced by another element in the model. + - They contain a site which is referenced by a :ref:`force` or :ref:`torque` sensor. + + This optimization is particularly useful when importing URDF models which often have many dummy bodies, but can also + be used to optimize MJCF models. After optimization, the new model has identical kinematics and dynamics as the + original but is faster to simulate. .. _compiler-inertiafromgeom: diff --git a/doc/changelog.rst b/doc/changelog.rst index 324d41e7..8efd9e54 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -18,6 +18,8 @@ General introduction in 3.3.1 of :ref:`tendon armature`. In addition to the traditional ``mjData.qM``, :ref:`mj_makeM` also computes ``mjData.M``, a CSR representation of the same matrix. - Added a new function :ref:`mj_copyBack` to copy real-valued arrays in an mjModel to a compatible mjSpec. +- Removed the limitation of :ref:`fusestatic` to models which contain no references. The fusestatic + flag will now fuse all bodies which are not referenced and ignore bodies which are referenced. Simulate ^^^^^^^^ diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 8d4e2b5a..8ce68cd7 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3824,24 +3824,77 @@ void mjCModel::FuseReindex(mjCBody* body) { +template +void mjCModel::ReassignChild(std::vector& dest, std::vector& list, + mjCBody* parent, mjCBody* body) { + for (int j=0; j < list.size(); j++) { + // assign + list[j]->body = parent; + dest.push_back(list[j]); + + // change frame + changeframe(list[j]->pos, list[j]->quat, body->pos, body->quat); + } + list.clear(); +} + + + +template +void mjCModel::ResolveReferences(std::vector& list, mjCBody* body) { + for (auto& item : list) { + item->CopyFromSpec(); + item->ResolveReferences(this); + } +} + + + +template <> +void mjCModel::ResolveReferences(std::vector& list, mjCBody* body) { + for (auto& item : list) { + item->CopyFromSpec(); + item->ResolveReferences(this); + } + for (mjCSensor* sensor : list) { + if (sensor->objtype == mjOBJ_SITE && + (sensor->type == mjSENS_FORCE || sensor->type == mjSENS_TORQUE) && + static_cast(sensor->obj)->body == body) { + throw mjCError(sensor, "cannot fuse a body used by a force/torque sensor"); + } + } +} + + + // fuse static bodies with their parent void mjCModel::FuseStatic(void) { - // skip if model has potential to reference elements with changed ids - if (!skins_.empty() || - !pairs_.empty() || - !excludes_.empty() || - !equalities_.empty() || - !tendons_.empty() || - !actuators_.empty() || - !sensors_.empty() || - !tuples_.empty() || - !cameras_.empty() || - !lights_.empty()) { - return; - } - - // process fusable bodies for (int i=1; i < bodies_.size(); i++) { + // check if the body can be fused + if (!bodies_[i]->name.empty()) { + ids[mjOBJ_BODY].erase(bodies_[i]->name); + + // try to resolve references without the name of this body, if it fails, skip + try { + ResolveReferences(cameras_); + ResolveReferences(lights_); + ResolveReferences(skins_); + ResolveReferences(pairs_); + ResolveReferences(excludes_); + ResolveReferences(equalities_); + ResolveReferences(tendons_); + ResolveReferences(actuators_); + ResolveReferences(sensors_, bodies_[i]); + ResolveReferences(tuples_); + } catch (mjCError err) { + ids[mjOBJ_BODY].insert({bodies_[i]->name, i}); + continue; + } + + // put body back the body name in the map + ids[mjOBJ_BODY].insert({bodies_[i]->name, i}); + } + // get body and parent mjCBody* body = bodies_[i]; mjCBody* par = body->parent; @@ -3891,25 +3944,8 @@ void mjCModel::FuseStatic(void) { //------------- assign geoms and sites to parent, change frames - // geoms - for (int j=0; j < body->geoms.size(); j++) { - // assign - body->geoms[j]->body = par; - par->geoms.push_back(body->geoms[j]); - - // change frame - changeframe(body->geoms[j]->pos, body->geoms[j]->quat, body->pos, body->quat); - } - - // sites - for (int j=0; j < body->sites.size(); j++) { - // assign - body->sites[j]->body = par; - par->sites.push_back(body->sites[j]); - - // change frame - changeframe(body->sites[j]->pos, body->sites[j]->quat, body->pos, body->quat); - } + ReassignChild(par->geoms, body->geoms, par, body); + ReassignChild(par->sites, body->sites, par, body); //------------- remove from global body list, reduce global counts @@ -3960,15 +3996,21 @@ void mjCModel::FuseStatic(void) { //------------- delete body (without deleting children) + // remove body name from map + if (!body->name.empty()) { + ids[mjOBJ_BODY].erase(body->name); + } + // delete allocation body->bodies.clear(); - body->geoms.clear(); - body->sites.clear(); delete body; // check index i again (we have a new body at this index) i--; } + + // remove empty names + processlist(ids, bodies_, mjOBJ_BODY, true); } @@ -4392,6 +4434,17 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { bodies_[i]->Compile(); // also compiles joints, geoms, sites, cameras, lights, frames } + // fuse static if enabled + if (compiler.fusestatic) { + FuseStatic(); + for (int i=0; i < lights_.size(); i++) { + lights_[i]->Compile(); + } + for (int i=0; i < cameras_.size(); i++) { + cameras_[i]->Compile(); + } + } + // compile all other objects except for keyframes for (auto flex : flexes_) flex->Compile(vfs); for (auto skin : skins_) skin->Compile(vfs); @@ -4423,10 +4476,6 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // resolve asset references, compute sizes IndexAssets(compiler.discardvisual); SetSizes(); - // fuse static if enabled - if (compiler.fusestatic) { - FuseStatic(); - } // set nmocap and body.mocapid for (mjCBody* body : bodies_) { diff --git a/src/user/user_model.h b/src/user/user_model.h index cfd8082e..4bb1927e 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -444,6 +444,14 @@ class mjCModel : public mjCModel_, private mjSpec { // generate a signature for the model uint64_t Signature(); + // reassign children of a body to a new parent + template + void ReassignChild(std::vector& dest, std::vector& list, mjCBody* parent, mjCBody* body); + + // resolve references in a list of objects + template + void ResolveReferences(std::vector& list, mjCBody* body = nullptr); + mjListKeyMap ids; // map from object names to ids mjCError errInfo; // last error info std::vector key_pending_; // attached keyframes diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 6a1b2451..d925a3a3 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3562,6 +3562,19 @@ void mjCCamera::CopyFromSpec() { +void mjCCamera::ResolveReferences(const mjCModel* m) { + if (!targetbody_.empty()) { + mjCBody* tb = (mjCBody*)m->FindObject(mjOBJ_BODY, targetbody_); + if (tb) { + targetbodyid = tb->id; + } else { + throw mjCError(this, "unknown target body in camera"); + } + } +} + + + // compiler void mjCCamera::Compile(void) { CopyFromSpec(); @@ -3587,14 +3600,7 @@ void mjCCamera::Compile(void) { mjuu_normvec(quat, 4); // get targetbodyid - if (!targetbody_.empty()) { - mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody_); - if (tb) { - targetbodyid = tb->id; - } else { - throw mjCError(this, "unknown target body in camera"); - } - } + ResolveReferences(model); // make sure the image size is finite if (fovy >= 180) { @@ -3716,6 +3722,27 @@ void mjCLight::CopyFromSpec() { +void mjCLight::ResolveReferences(const mjCModel* m) { + if (!targetbody_.empty()) { + mjCBody* tb = (mjCBody*)m->FindObject(mjOBJ_BODY, targetbody_); + if (tb) { + targetbodyid = tb->id; + } else { + throw mjCError(this, "unknown target body in light"); + } + } + if (!texture_.empty()) { + mjCTexture* tex = (mjCTexture*)m->FindObject(mjOBJ_TEXTURE, texture_); + if (tex) { + texid = tex->id; + } else { + throw mjCError(this, "unknown texture in light"); + } + } +} + + + // compiler void mjCLight::Compile(void) { CopyFromSpec(); @@ -3735,25 +3762,8 @@ void mjCLight::Compile(void) { throw mjCError(this, "zero direction in light"); } - // get targetbodyid - if (!targetbody_.empty()) { - mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody_); - if (tb) { - targetbodyid = tb->id; - } else { - throw mjCError(this, "unknown target body in light"); - } - } - - // get texture - if (!texture_.empty()) { - mjCTexture* tex = (mjCTexture*)model->FindObject(mjOBJ_TEXTURE, texture_); - if (tex) { - texid = tex->id; - } else { - throw mjCError(this, "unknown target body in light"); - } - } + // get targetbodyid and texid + ResolveReferences(model); } @@ -6493,6 +6503,8 @@ void mjCSensor::CopyPlugin() { void mjCSensor::ResolveReferences(const mjCModel* m) { + obj = nullptr; + ref = nullptr; objname_ = prefix + objname_ + suffix; refname_ = prefix + refname_ + suffix; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 18158c5c..fc8cf459 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -786,6 +786,7 @@ class mjCCamera : public mjCCamera_, private mjsCamera { void CopyFromSpec(void); void PointToLocal(void); void NameSpace(const mjCModel* m); + void ResolveReferences(const mjCModel* m); }; @@ -831,6 +832,7 @@ class mjCLight : public mjCLight_, private mjsLight { void CopyFromSpec(void); void PointToLocal(void); void NameSpace(const mjCModel* m); + void ResolveReferences(const mjCModel* m); }; diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 8b00753d..3293b257 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -440,6 +440,100 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { mj_deleteModel(m_no_fuse); } +TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) { + static constexpr char xml_template[] = R"( + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; + mj_deleteModel(m); +} + +TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) { + static constexpr char xml_template[] = R"( + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; + mj_deleteModel(m); +} + +TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) { + static constexpr char xml_template[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; + mj_deleteModel(m); +} + // ------------- test discardvisual -------------------------------------------- using DiscardVisualTest = MujocoTest;