Increment body node version after it has been removed from a new skel…

…eton (#1489)

Co-authored-by: Grey <[email protected]>

CHANGELOG.md

@@ -28,6 +28,7 @@
   * Added secondary friction coefficient parameter: [#1424](https://github.com/dartsim/dart/pull/1424)
   * Allowed to set friction direction per ShapeFrame: [#1427](https://github.com/dartsim/dart/pull/1427)
   * Fixed incorrect vector resizing in BoxedLcpConstraintSolver: [#1459](https://github.com/dartsim/dart/pull/1459)
+  * Changed to increment BodyNode version when it's being removed from Skeleton: [#1489](https://github.com/dartsim/dart/pull/1489)
 
 * GUI
 

dart/dynamics/Skeleton.cpp

@@ -2267,6 +2267,9 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode)
 
   _newBodyNode->incrementVersion();
   _newBodyNode->mStructuralChangeSignal.raise(_newBodyNode);
+  // We don't need to explicitly increment the version of this Skeleton here
+  // because the BodyNode will increment the version of its dependent, which is
+  // this Skeleton.
 }
 
 //==============================================================================
@@ -2438,6 +2441,12 @@ void Skeleton::unregisterBodyNode(BodyNode* _oldBodyNode)
   }
 
   updateTotalMass();
+
+  _oldBodyNode->incrementVersion();
+  _oldBodyNode->mStructuralChangeSignal.raise(_oldBodyNode);
+  // We don't need to explicitly increment the version of this Skeleton here
+  // because the BodyNode will increment the version of its dependent, which is
+  // this Skeleton.
 }
 
 //==============================================================================

unittests/comprehensive/test_Skeleton.cpp

@@ -144,7 +144,11 @@ TEST(Skeleton, Restructuring)
     BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2;
     BodyNode* parent = child == bn1 ? bn2 : bn1;
 
+    auto skelVer = skeleton->getVersion();
+    auto childVer = child->getVersion();
     child->moveTo(parent);
+    EXPECT_NE(skeleton->getVersion(), skelVer);
+    EXPECT_NE(child->getVersion(), childVer);
 
     EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
     if (skeleton->getNumBodyNodes() == original->getNumBodyNodes())
@@ -263,7 +267,13 @@ TEST(Skeleton, Restructuring)
     constructSubtree(subtree, childBn);
 
     // Move to a new Skeleton
+    auto fromSkelVer = fromSkel->getVersion();
+    auto toSkelVer = toSkel->getVersion();
+    auto child_ver = childBn->getVersion();
     childBn->moveTo(parentBn);
+    EXPECT_NE(fromSkel->getVersion(), fromSkelVer);
+    EXPECT_NE(toSkel->getVersion(), toSkelVer);
+    EXPECT_NE(childBn->getVersion(), child_ver);
     EXPECT_TRUE(childBn->getSkeleton()->checkIndexingConsistency());
     EXPECT_TRUE(parentBn->getSkeleton()->checkIndexingConsistency());
 

unittests/regression/test_Issue1445.cpp

@@ -74,8 +74,8 @@ TEST(Issue1445, Collision)
 
   auto world = std::make_shared<dart::simulation::World>();
 
+  auto ground = dart::dynamics::Skeleton::create("ground");
   {
-    auto ground = dart::dynamics::Skeleton::create("ground");
     auto* bn = ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
                    .second;
     bn->createShapeNodeWith<
@@ -153,14 +153,25 @@ TEST(Issue1445, Collision)
   EXPECT_NEAR(0.0, model1Body->getLinearVelocity().z(), 1e-3);
   EXPECT_NEAR(0.0, model2Body->getLinearVelocity().z(), 1e-3);
 
-  auto temp = dart::dynamics::Skeleton::create("temp");
-  world->addSkeleton(temp);
-  model2Body->moveTo<dart::dynamics::FreeJoint>(temp, nullptr);
+  auto temp1 = dart::dynamics::Skeleton::create("temp1");
+  world->addSkeleton(temp1);
+  model2Body->moveTo<dart::dynamics::FreeJoint>(temp1, nullptr);
 
   for (std::size_t i = 0; i < numSteps; ++i)
     world->step();
 
   // Expect both bodies to remain in contact with the ground with zero velocity.
   EXPECT_NEAR(0.0, model1Body->getLinearVelocity().z(), 1e-3);
   EXPECT_NEAR(0.0, model2Body->getLinearVelocity().z(), 1e-3);
+
+  auto* groundBody = ground->getRootBodyNode();
+  auto temp2 = groundBody->remove();
+
+  for (std::size_t i = 0; i < numSteps; ++i)
+    world->step();
+
+  // Expect both bodies to be falling after the BodyNode ofthe the ground is
+  // removed
+  EXPECT_LE(model1Body->getLinearVelocity().z(), -1e-2);
+  EXPECT_LE(model2Body->getLinearVelocity().z(), -1e-2);
 }