Nested Models: resolve joints for nested models

- Add flag to Modellnfo to mark if the model is nested.
- Update EntityManagementFeatures::GetJoint to handle duplicate (unscoped) joint names.
- Update SDFFeatures::ConstructSdfJoint to scope joint names for nested models

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

dartsim/src/Base.hh

@@ -83,6 +83,7 @@ struct ModelInfo
   std::string canonicalLinkName;
   std::vector<std::shared_ptr<LinkInfo>> links {};
   std::vector<std::size_t> nestedModels = {};
+  bool isNested{false};
 };
 
 struct JointInfo
@@ -331,6 +332,8 @@ class Base : public Implements3d<FeatureList<Feature>>
     this->models.idToContainerID[id] = _parentID;
     this->frames[id] = _info.frame.get();
     parentModelInfo->nestedModels.push_back(id);
+    entry.isNested = true;
+
     return {id, entry};
   }
 

dartsim/src/EntityManagementFeatures.cc

@@ -461,9 +461,17 @@ Identity EntityManagementFeatures::GetJoint(
 Identity EntityManagementFeatures::GetJoint(
     const Identity &_modelID, const std::string &_jointName) const
 {
-  DartJoint *const joint =
-      this->ReferenceInterface<ModelInfo>(_modelID)->model->getJoint(
-          _jointName);
+  // Handle joints for nested models.
+  auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+
+  // See: SDFFeatures::ConstructSdfJoint
+  std::string jointName = _jointName;
+  if (modelInfo->isNested)
+  {
+    jointName = ::sdf::JoinName(modelInfo->model->getName(), jointName);
+  }
+
+  DartJoint *const joint = modelInfo->model->getJoint(jointName);
 
   // If the model does not contain the joint, it may be because it is a
   // nested model in which case the joint is associated with the top-level
@@ -480,7 +488,17 @@ Identity EntityManagementFeatures::GetJoint(
       {
         auto parentID = this->GenerateIdentity(parentIdx,
             this->models.at(parentIdx));
-        return this->GetJoint(parentID, _jointName);
+        auto parentInfo = this->ReferenceInterface<ModelInfo>(parentID);
+
+        // If the parent is nested, then recurse using the un-prefixed
+        // joint name as GetJoint will apply a prefix.
+        // If the parent is not nested, recurse using the prefixed joint name
+        // to avoid name conflicts in cases where the parent has a joint
+        //  with the same unscoped name as the nested model.
+        if (parentInfo->isNested)
+          return this->GetJoint(parentID, _jointName);
+        else
+          return this->GetJoint(parentID, jointName);
       }
     }
     return this->GenerateInvalidId();
@@ -493,7 +511,7 @@ Identity EntityManagementFeatures::GetJoint(
     const std::size_t jointID = this->joints.IdentityOf(joint);
 
     // debug(srmainwaring)
-    gzmsg << "DartSim: Found joint named ["
+    gzmsg << "Found joint named ["
         << _jointName << "] for modelID [" << _modelID.id << "], "
         << "jointID: " << jointID << "\n";
 

dartsim/src/SDFFeatures.cc

@@ -512,6 +512,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
   {
     this->ConstructSdfModelImpl(modelID, *_sdfModel.ModelByIndex(i));
   }
+
   // then, construct all links
   for (std::size_t i=0; i < _sdfModel.LinkCount(); ++i)
   {
@@ -1207,7 +1208,21 @@ Identity SDFFeatures::ConstructSdfJoint(
     joint = _child->moveTo<dart::dynamics::WeldJoint>(_parent);
   }
 
-  joint->setName(_sdfJoint.Name());
+
+  /// \todo(srmainwaring) update name for nested models - similar to
+  /// ConstructSdfModelImpl
+  std::string jointName = _sdfJoint.Name();
+
+  if (_modelInfo.isNested)
+  {
+    jointName = ::sdf::JoinName(_modelInfo.model->getName(), jointName);
+  }
+
+  gzdbg << "Created DART joint: [" << joint->getName() << "]\n";
+
+  joint->setName(jointName);
+
+  gzdbg << "Renamed DART joint: [" << joint->getName() << "]\n";
 
   const Eigen::Isometry3d child_T_postjoint = T_child.inverse() * T_joint;
   const Eigen::Isometry3d parent_T_prejoint_init = T_parent.inverse() * T_joint;
@@ -1222,11 +1237,11 @@ Identity SDFFeatures::ConstructSdfJoint(
   _child->incrementVersion();
 
   // debug(srmainwaring)
-  // {
-  //   gzdbg << "SDFFeatures::ConstructSdfJoint: On Exit\n"
-  //         << this->DebugModels()
-  //         << this->DebugJoints() << "\n";
-  // }
+  {
+    gzdbg << "SDFFeatures::ConstructSdfJoint: On Exit\n"
+          << this->DebugModels()
+          << this->DebugJoints() << "\n";
+  }
 
   return this->GenerateIdentity(jointID, this->joints.at(jointID));
 }