Patch for double-delete issue reported by #671

CMakeLists.txt

@@ -24,7 +24,7 @@ project(dart)
 
 set(DART_MAJOR_VERSION "4")
 set(DART_MINOR_VERSION "3")
-set(DART_PATCH_VERSION "5")
+set(DART_PATCH_VERSION "6")
 set(DART_VERSION "${DART_MAJOR_VERSION}.${DART_MINOR_VERSION}.${DART_PATCH_VERSION}")
 set(DART_PKG_DESC "Dynamic Animation and Robotics Toolkit.")
 set(DART_PKG_EXTERNAL_DEPS "flann, ccd, fcl")

dart/dynamics/Skeleton.cpp

@@ -236,6 +236,13 @@ void Skeleton::addBodyNode(BodyNode* _body)
 {
   assert(_body && _body->getParentJoint());
 
+  if(std::find(mBodyNodes.begin(), mBodyNodes.end(), _body)
+     != mBodyNodes.end())
+  {
+    // To help with #671, we prevent BodyNodes from having duplicate entries
+    return;
+  }
+
   mBodyNodes.push_back(_body);
   addEntryToBodyNodeNameMgr(_body);
   addMarkersOfBodyNode(_body);
@@ -428,7 +435,21 @@ void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity)
   for (size_t i = 0; i < mBodyNodes.size(); ++i)
   {
     if (mBodyNodes[i]->getParentBodyNode() == NULL)
+    {
+      if(std::find(rootBodyNodes.begin(), rootBodyNodes.end(), mBodyNodes[i])
+         != rootBodyNodes.end())
+      {
+        // Avoid duplicate BodyNodes
+        dtmsg << "[Skeleton::init] The root BodyNode named ["
+              << mBodyNodes[i]->getName() << "] (" << mBodyNodes[i] << ") "
+              << "appears twice while initializing the Skeleton named ["
+              << getName() << "] (" << this << "). The second appearance will "
+              << "be ignored.\n";
+        continue;
+      }
+
       rootBodyNodes.push_back(mBodyNodes[i]);
+    }
   }
 
   // Rearrange the list of body nodes with BFS (Breadth First Search)
@@ -442,9 +463,29 @@ void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity)
     {
       BodyNode* itBodyNode = queue.front();
       queue.pop();
+
+      if(std::find(mBodyNodes.begin(), mBodyNodes.end(), itBodyNode)
+         != mBodyNodes.end())
+      {
+        // Addressing issue #671
+        const BodyNode* parent = itBodyNode->getParentBodyNode();
+        dtwarn << "[Skeleton::init] The BodyNode named ["
+               << itBodyNode->getName() << "] (" << itBodyNode << ") is being "
+               << "claimed by multiple parents. This is not allowed! It will "
+               << "only use [" << parent->getName() << "] (" << parent << ") "
+               << "as its parent!\n";
+        continue;
+      }
+
       mBodyNodes.push_back(itBodyNode);
       for (size_t j = 0; j < itBodyNode->getNumChildBodyNodes(); ++j)
-        queue.push(itBodyNode->getChildBodyNode(j));
+      {
+        BodyNode* child = itBodyNode->getChildBodyNode(j);
+        queue.push(child);
+
+        // Addressing issue #671
+        child->mParentBodyNode = itBodyNode;
+      }
     }
   }
 

unittests/testBuilding.cpp

@@ -130,6 +130,39 @@ TEST(BUILDING, BASIC)
     delete world;
 }
 
+/******************************************************************************/
+TEST(BUILDING, DUPLICATION)
+{
+  BodyNode* body1 = new BodyNode;
+  BodyNode* body2 = new BodyNode;
+  BodyNode* child = new BodyNode;
+
+  RevoluteJoint* joint1 = new RevoluteJoint;
+  RevoluteJoint* joint2 = new RevoluteJoint;
+  RevoluteJoint* joint3 = new RevoluteJoint;
+
+  body1->setParentJoint(joint1);
+  body2->setParentJoint(joint2);
+  child->setParentJoint(joint3);
+
+  body1->addChildBodyNode(child);
+  body2->addChildBodyNode(child);
+
+  Skeleton* skel = new Skeleton;
+  skel->addBodyNode(body1);
+  skel->addBodyNode(body1);
+  skel->addBodyNode(body2);
+  skel->addBodyNode(child);
+
+  skel->init();
+
+  // Because of how the init queue works, it will use the last BodyNode in the
+  // list of Skeleton::mBodyNodes which claims it as its child.
+  EXPECT_EQ(child->getParentBodyNode(), body2);
+
+  delete skel;
+}
+
 /******************************************************************************/
 int main(int argc, char* argv[])
 {