diff --git a/dartsim/src/KinematicsFeatures.cc b/dartsim/src/KinematicsFeatures.cc index bc1f0711c..37b7fce5c 100644 --- a/dartsim/src/KinematicsFeatures.cc +++ b/dartsim/src/KinematicsFeatures.cc @@ -64,14 +64,6 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( const FrameID &_id) const { - const auto model_it = this->models.idToObject.find(_id.ID()); - if (model_it != this->models.idToObject.end()) - { - // This is a model FreeGroup frame, so we'll use the first root link as the - // frame - return model_it->second->model->getRootBodyNode(); - } - auto framesIt = this->frames.find(_id.ID()); if (framesIt == this->frames.end()) { diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index c2f047513..5d15a442b 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -217,11 +217,8 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0); auto modelFrameData = model->FrameDataRelativeToWorld(); - if (this->PhysicsEngineName(name) == "bullet-featherstone") - { - EXPECT_EQ(actualModelPose, - gz::math::eigen3::convert(modelFrameData.pose)); - } + EXPECT_EQ(actualModelPose, + gz::math::eigen3::convert(modelFrameData.pose)); auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld(); auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose); gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0); @@ -254,14 +251,11 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) linkColPose.Rot().Euler()); gz::math::Pose3d actualNestedModelLocalPose(0, 0, 1, 0, 0, 0); - if (this->PhysicsEngineName(name) == "bullet-featherstone") - { - auto nestedModelFrameData = nestedModel->FrameDataRelativeToWorld(); - gz::math::Pose3d expectedNestedModelWorldPose = - actualModelPose * actualNestedModelLocalPose; - EXPECT_EQ(expectedNestedModelWorldPose, - gz::math::eigen3::convert(nestedModelFrameData.pose)); - } + auto nestedModelFrameData = nestedModel->FrameDataRelativeToWorld(); + gz::math::Pose3d expectedNestedModelWorldPose = + actualModelPose * actualNestedModelLocalPose; + EXPECT_EQ(expectedNestedModelWorldPose, + gz::math::eigen3::convert(nestedModelFrameData.pose)); auto nestedLinkFrameData = nestedLink->FrameDataRelativeToWorld(); auto nestedLinkPose = gz::math::eigen3::convert(nestedLinkFrameData.pose);