Fix construction of nested models (#445)

* Fix construction of nested models

Signed-off-by: Michael Carroll <[email protected]>

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);