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 + + + + + + +