Nested Models: search parent models for joints.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
srmainwaring · Dec 27, 2022 · b9b3ffd · b9b3ffd
1 parent f318d4b
commit b9b3ffd
Show file tree

Hide file tree

Showing 2 changed files with 48 additions and 3 deletions.
diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh
@@ -607,7 +607,7 @@ class Base : public Implements3d<FeatureList<Feature>>
            << "NumJoints:   " << modelInfo->model->getNumJoints() << "\n";
         for (auto& joint :  modelInfo->model->getJoints())
         {
-          ss  << "  Joint:  " << joint->getName() << "\n";
+          ss  << "  Joint:     " << joint->getName() << "\n";
         }
       }
     }

diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc
@@ -415,6 +415,27 @@ Identity EntityManagementFeatures::GetJoint(
       this->ReferenceInterface<ModelInfo>(_modelID)->model->getJoint(
           _jointIndex);
 
+  // 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
+  // model (the DART skeleton that all joints have been moved to).
+  if (joint == nullptr)
+  {
+    // Call GetJoint recursively until we either find a model that contains
+    // the joint or we reach the top level model (which has no container).
+    if (this->models.idToContainerID.find(_modelID.id) !=
+        this->models.idToContainerID.end())
+    {
+      auto parentIdx = this->models.idToContainerID.at(_modelID.id);
+      if (this->models.HasEntity(parentIdx))
+      {
+        auto parentID = this->GenerateIdentity(parentIdx,
+            this->models.at(parentIdx));
+        return this->GetJoint(parentID, _jointIndex);
+      }
+    }
+    return this->GenerateInvalidId();
+  }
+
   // If the joint doesn't exist in "joints", it means the containing entity has
   // been removed.
   if (this->joints.HasEntity(joint))
@@ -428,6 +449,10 @@ Identity EntityManagementFeatures::GetJoint(
     // model that has been removed. Right now we are returning an invalid
     // identity, but that could cause a segfault if the use doesn't check if
     // returned value before using it.
+
+    gzwarn << "No joint with ID ["
+        << _jointIndex << "] for modelID [" << _modelID.id << "]\n";
+
     return this->GenerateInvalidId();
   }
 }
@@ -440,6 +465,27 @@ Identity EntityManagementFeatures::GetJoint(
       this->ReferenceInterface<ModelInfo>(_modelID)->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
+  // model (the DART skeleton that all joints have been moved to).
+  if (joint == nullptr)
+  {
+    // Call GetJoint recursively until we either find a model that contains
+    // the joint or we reach the top level model (which has no container).
+    if (this->models.idToContainerID.find(_modelID.id) !=
+        this->models.idToContainerID.end())
+    {
+      auto parentIdx = this->models.idToContainerID.at(_modelID.id);
+      if (this->models.HasEntity(parentIdx))
+      {
+        auto parentID = this->GenerateIdentity(parentIdx,
+            this->models.at(parentIdx));
+        return this->GetJoint(parentID, _jointName);
+      }
+    }
+    return this->GenerateInvalidId();
+  }
+
   // If the joint doesn't exist in "joints", it means the containing entity has
   // been removed.
   if (this->joints.HasEntity(joint))
@@ -467,8 +513,7 @@ Identity EntityManagementFeatures::GetJoint(
     // identity, but that could cause a segfault if the use doesn't check if
     // returned value before using it.
 
-    // debug(srmainwaring)
-    gzwarn << "DartSim: No joint named ["
+    gzwarn << "No joint named ["
         << _jointName << "] for modelID [" << _modelID.id << "]\n";
 
     // debug(srmainwaring)