diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index cd5d0d6dd..832839b84 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1205,7 +1205,24 @@ bool SDFFeatures::AddSdfCollision( auto *world = this->ReferenceInterface(model->world); - if (isStatic) + // Set static filter for collisions in + // 1) a static model + // 2) a fixed base link + // 3) a (non-base) link with zero dofs + bool isFixed = false; + if (model->body->hasFixedBase()) + { + // check if it's a base link + isFixed = std::size_t(_linkID) == + static_cast(model->body->getUserIndex()); + // check if link has zero dofs + if (!isFixed && linkInfo->indexInModel.has_value()) + { + isFixed = model->body->getLink( + linkInfo->indexInModel.value()).m_dofCount == 0; + } + } + if (isStatic || isFixed) { world->world->addCollisionObject( linkInfo->collider.get(), diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index f78495126..db2567e41 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -23,6 +24,7 @@ #include "test/Resources.hh" #include "test/TestLibLoader.hh" +#include "Worlds.hh" #include #include @@ -30,14 +32,18 @@ #include #include #include +#include #include #include #include #include +#include #include #include +#include + template class CollisionTest: public testing::Test, public gz::physics::TestLibLoader @@ -141,6 +147,125 @@ TYPED_TEST(CollisionTest, MeshAndPlane) } } +using CollisionStaticFeaturesList = gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetContactsFromLastStepFeature, + gz::physics::ForwardStep +>; + +using CollisionStaticTestFeaturesList = + CollisionTest; + +TEST_F(CollisionStaticTestFeaturesList, StaticCollisions) +{ + auto getBoxStaticStr = [](const std::string &_name, + const gz::math::Pose3d &_pose) + { + std::stringstream modelStaticStr; + modelStaticStr << R"( + + + )"; + modelStaticStr << _pose; + modelStaticStr << R"( + + + + 1 1 1 + + + + true + + )"; + return modelStaticStr.str(); + }; + + auto getBoxFixedJointStr = [](const std::string &_name, + const gz::math::Pose3d &_pose) + { + std::stringstream modelFixedJointStr; + modelFixedJointStr << R"( + + + )"; + modelFixedJointStr << _pose; + modelFixedJointStr << R"( + + + + 1 1 1 + + + + + world + body + + + )"; + return modelFixedJointStr.str(); + }; + + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + sdf::Root rootWorld; + const sdf::Errors errorsWorld = + rootWorld.Load(common_test::worlds::kGroundSdf); + ASSERT_TRUE(errorsWorld.empty()) << errorsWorld.front(); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + ASSERT_NE(nullptr, engine); + + auto world = engine->ConstructWorld(*rootWorld.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(getBoxStaticStr( + "box_static", gz::math::Pose3d::Zero)); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + for (std::size_t i = 0; i < 10; ++i) + { + world->Step(output, state, input); + } + + // static box overlaps with ground plane + // verify no contacts between static bodies. + auto contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(0u, contacts.size()); + + // currently only bullet-featherstone skips collision checking between + // static bodies and bodies with world fixed joint + if (this->PhysicsEngineName(name) != "bullet-featherstone") + continue; + + errors = root.LoadSdfString(getBoxFixedJointStr( + "box_fixed_world_joint", gz::math::Pose3d::Zero)); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + world->Step(output, state, input); + // box fixed to world overlaps with static box and ground plane + // verify there are still no contacts. + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(0u, contacts.size()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv);