diff --git a/CHANGELOG.md b/CHANGELOG.md index fc7c125e889c4..b4f61b71653f4 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 3208ca5ced212..f46a67509c6fe 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/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. } //============================================================================== diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index 50a7131376532..a6dbda5ceec5c 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/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()); diff --git a/unittests/regression/test_Issue1445.cpp b/unittests/regression/test_Issue1445.cpp index b6072fd113a85..8fa5c6e89edf0 100644 --- a/unittests/regression/test_Issue1445.cpp +++ b/unittests/regression/test_Issue1445.cpp @@ -74,8 +74,8 @@ TEST(Issue1445, Collision) auto world = std::make_shared(); + auto ground = dart::dynamics::Skeleton::create("ground"); { - auto ground = dart::dynamics::Skeleton::create("ground"); auto* bn = ground->createJointAndBodyNodePair() .second; bn->createShapeNodeWith< @@ -153,9 +153,9 @@ 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(temp, nullptr); + auto temp1 = dart::dynamics::Skeleton::create("temp1"); + world->addSkeleton(temp1); + model2Body->moveTo(temp1, nullptr); for (std::size_t i = 0; i < numSteps; ++i) world->step(); @@ -163,4 +163,15 @@ TEST(Issue1445, Collision) // 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); }