diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index 74c752c0d..6470a0c44 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -39,12 +39,6 @@ Identity SDFFeatures::ConstructSdfModel( // const auto &world = this->worlds.at(_worldID)->world; const auto modelIdentity = this->AddModel({name, _worldID, isStatic, pose}); - // Build links - for (std::size_t i = 0; i < _sdfModel.LinkCount(); ++i) - { - this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i)); - } - return modelIdentity; } @@ -172,6 +166,27 @@ Identity SDFFeatures::ConstructSdfCollision( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +std::size_t SDFFeatures::FindOrConstructSdfLink( + const Identity &_modelID, + const ::sdf::Link &_sdfLink) +{ + // Check if there is a model with the requested name + const std::string linkName = _sdfLink.Name(); + + for (const auto &link : this->links) + { + const auto &linkInfo = link.second; + if (linkInfo->name == linkName) + { + // A link was previously created with that name, + // Return its entity value + return link.first; + } + } + return this->ConstructSdfLink(_modelID, _sdfLink); +} + } } } diff --git a/bullet/src/SDFFeatures.hh b/bullet/src/SDFFeatures.hh index 3202c05e4..ac40b3e35 100644 --- a/bullet/src/SDFFeatures.hh +++ b/bullet/src/SDFFeatures.hh @@ -40,6 +40,9 @@ class SDFFeatures : private: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) override; + private: std::size_t FindOrConstructSdfLink( + const Identity &_modelID, + const ::sdf::Link &_sdfLink); }; }