From 0860af05d50635acd333a8af2eefd47369d551d7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Jun 2021 14:38:02 -0500 Subject: [PATCH] Fix collision detection issue when using joints across nested models (#268) When a joint is created between BodyNodes in different skeletons, the child BodyNode is moved to the skeleton of the parent BodyNode. When this happens, the BodyNode version of the child needs to be incremented. This is actually fixed by dartsim/dart#1445, but we don't have that merged into our fork. So in the meantime, we call incrementVersion after moveTo is called similar to #31. Signed-off-by: Addisu Z. Taddese Co-authored-by: Louise Poubel --- dartsim/src/SDFFeatures.cc | 5 + dartsim/src/SDFFeatures_TEST.cc | 52 +++++++ dartsim/worlds/joint_across_nested_models.sdf | 127 ++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 dartsim/worlds/joint_across_nested_models.sdf diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 3dc16fc43..27be8828b 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -1140,6 +1140,11 @@ Identity SDFFeatures::ConstructSdfJoint( joint->setTransformFromParentBodyNode(parent_T_prejoint_final); const std::size_t jointID = this->AddJoint(joint); + // Increment BodyNode version since the child could be moved to a new skeleton + // when a joint is created. + // TODO(azeey) Remove incrementVersion once DART has been updated to + // internally increment the BodyNode's version after Joint::moveTo. + _child->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 4d25c84fe..320bbed87 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -30,6 +30,7 @@ #include +#include #include #include #include @@ -57,6 +58,7 @@ struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::GetEntities, ignition::physics::GetBasicJointState, ignition::physics::SetBasicJointState, + ignition::physics::LinkFrameSemantics, ignition::physics::dartsim::RetrieveWorld, ignition::physics::sdf::ConstructSdfCollision, ignition::physics::sdf::ConstructSdfJoint, @@ -661,6 +663,56 @@ TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) } } +///////////////////////////////////////////////// +// Check that joints between links in different models work as expected +TEST_P(SDFFeatures_TEST, JointsAcrossNestedModels) +{ + WorldPtr world = this->LoadWorld( + TEST_WORLD_DIR "/joint_across_nested_models.sdf"); + ASSERT_NE(nullptr, world); + + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + + auto checkModel = [&world](const std::string &_modelName){ + SCOPED_TRACE("checkModel " + _modelName); + // check top level model + auto parentModel = world->GetModel(_modelName); + ASSERT_NE(nullptr, parentModel); + + auto link1 = parentModel->GetLink("link1"); + ASSERT_NE(nullptr, link1); + + auto nestedModel = parentModel->GetNestedModel("nested_model"); + ASSERT_NE(nullptr, nestedModel); + + auto link2 = nestedModel->GetLink("link2"); + ASSERT_NE(nullptr, link2); + + Eigen::Vector3d link1Pos = + link1->FrameDataRelativeToWorld().pose.translation(); + Eigen::Vector3d link2Pos = + link2->FrameDataRelativeToWorld().pose.translation(); + EXPECT_NEAR(0.25, link1Pos.z(), 1e-6); + EXPECT_NEAR(0.25, link2Pos.z(), 1e-6); + }; + + { + SCOPED_TRACE("Before step"); + checkModel("M1"); + checkModel("M2"); + } + for (int i = 0; i < 1000; ++i) + { + dartWorld->step(); + } + { + SCOPED_TRACE("After step"); + checkModel("M1"); + checkModel("M2"); + } +} + ///////////////////////////////////////////////// class SDFFeatures_FrameSemantics: public SDFFeatures_TEST { diff --git a/dartsim/worlds/joint_across_nested_models.sdf b/dartsim/worlds/joint_across_nested_models.sdf new file mode 100644 index 000000000..fb4e8c8b4 --- /dev/null +++ b/dartsim/worlds/joint_across_nested_models.sdf @@ -0,0 +1,127 @@ + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + + + + + 0 0 0.25 0 0.0 0 + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + link1 + nested_model::link2 + + + + 1 0 0.0 0 0.0 0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + + + 0 5 0.25 0 0.0 0 + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + nested_model::link2 + link1 + + + + 1.0 0 0.0 0 0.0 0 + + + + + 1 1 0.5 + + + + + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + +