diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index b76ac9ba3..4fbe181dc 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -441,8 +441,23 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (isNested) { worldID = this->GetWorldOfModelImpl(_parentID); - const auto &skel = this->models.at(_parentID)->model; + + const auto parentModelInfo = this->models.at(_parentID); + const auto &skel = parentModelInfo->model; modelName = ::sdf::JoinName(skel->getName(), _sdfModel.Name()); + + // Check to see if the nested model has already been constructed. + // This can happen in the case that it was recursively created as + // part of the parent model. + // In this case, return the identity, rather than duplicating. + for (const auto &nestedModelID : parentModelInfo->nestedModels) + { + auto nestedModel = this->models.at(nestedModelID); + if (nestedModel->localName == _sdfModel.Name()) + { + return this->GenerateIdentity(nestedModelID, nestedModel); + } + } } dart::dynamics::Frame *parentFrame = this->frames.at(_parentID);