Duplicate entries in Skeleton::mBodyNodes causing segfault in destructor

[chapulina]

On Skeleton::Init, the vector of body nodes within the skeleton is rearranged with breadth first search. During the rearrangement, there are circumstances in which the same node gets added twice. For example, if the same body node is the child of two different joints.

This causes a segfault on the skeleton's destructor, where it tries to delete the same pointer twice.

I was able to avoid the crash with the following patch, but I'm not sure if this would break other things.

diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
index 9e8eb3f..3f6d0be 100644
--- a/dart/dynamics/Skeleton.cpp
+++ b/dart/dynamics/Skeleton.cpp
@@ -442,7 +442,8 @@ void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity)
     {
       BodyNode* itBodyNode = queue.front();
       queue.pop();
-      mBodyNodes.push_back(itBodyNode);
+      if (std::find(mBodyNodes.begin(), mBodyNodes.end(), itBodyNode) == mBodyNodes.end())
+        mBodyNodes.push_back(itBodyNode);
       for (size_t j = 0; j < itBodyNode->getNumChildBodyNodes(); ++j)
         queue.push(itBodyNode->getChildBodyNode(j));
     }

The issue was discovered while debugging a test failure in Gazebo where the skeleton's destructor is called, see this pull request.

[chapulina]

@scpeters

[jslee02]

Presumably, the test tries to parse a model (maybe pr2.world?) that includes kinematic closed loop. The closed loop constraint should be handled as a dynamic constraint, but as reported here, DART currently doesn't support automatic closed loop constraint construction yet.

This patch would resolve the segfault by ignoring the loops, but the test will possibly fail anyways since the closed loop constraint will be violated.

I think the test in Gazebo should be disabled until this is resolved in DART.

By the way, thanks for reminding us that DART is missing this feature.

See also: #465

[mxgrey]

The Skeleton::init function no longer exists in versions >= 5.0, because Skeleton initialization is now handled automatically. We could patch version 4.3 if Gazebo users still need it.

This does raise some red flags in my mind, though. The only ways I could imagine this happening are:

1. Two BodyNodes claim to have the same child BodyNode. That would result in a non-tree structure, which is not supported by DART.
2. The same BodyNode has been added to the tree twice. This wouldn't necessarily break the dynamics algorithms, but it would probably be indicative of an error somewhere else.

Rather than silently handle and ignore this situation as proposed, I think we should patch it to print a warning to the user that they have provided a malformed Skeleton.

[scpeters]

Yes, we discovered this issue while simulating the pr2 (joint_screw.cc). That test is actually already disabled for dart, but it doesn't exit until after loading the pr2 world, and it seg-faults during the destructor. Due to memory management issues in gazebo (shared pointer reference loops), the destructors weren't being called until recently. For now, we can keep the test disabled, but make sure it doesn't even load the PR2 model.

[mxgrey]

I think it's reasonable to patch DART v4.3 so that it prints a warning and dodges the segfault. The resulting tree just won't reflect the original intent of the model, because it will not close that loop. I'll make a PR for this.

[mxgrey]

The patch has been merged in #672