From 10238aa3b18b1767023f424ccc268e186db2259b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 27 Mar 2024 23:16:15 +0000 Subject: [PATCH 1/3] ignore collision between static link and base link with world joint Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 7 +- test/common_test/collisions.cc | 130 +++++++++++++++++++++++++ 2 files changed, 136 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index cd5d0d6dd..3efc905f9 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1205,7 +1205,12 @@ bool SDFFeatures::AddSdfCollision( auto *world = this->ReferenceInterface(model->world); - if (isStatic) + // Set static filter for collisions in a static model + // and collisions in a base link that is fixed to the world + std::size_t linkID = model->body->getUserIndex(); + bool isFixedBaseLink = model->body->hasFixedBase() && + (std::size_t(_linkID) == linkID); + if (isStatic || isFixedBaseLink) { world->world->addCollisionObject( linkInfo->collider.get(), diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index f78495126..9c16cd3ef 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,130 @@ TYPED_TEST(CollisionTest, MeshAndPlane) } } +struct CollisionStaticFeaturesList : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetContactsFromLastStepFeature, + gz::physics::ForwardStep +> { }; + +template +class CollisionStaticTest : + public CollisionTest{}; +using CollisionStaticTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(CollisionStaticTest, + CollisionStaticTestTypes); + +TYPED_TEST(CollisionStaticTest, 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); From 9377b052edacb49d8dd47ee736d2e409dd073e9e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 29 Mar 2024 22:56:30 +0000 Subject: [PATCH 2/3] check link with zero dof Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 3efc905f9..e50c8564b 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1205,12 +1205,24 @@ bool SDFFeatures::AddSdfCollision( auto *world = this->ReferenceInterface(model->world); - // Set static filter for collisions in a static model - // and collisions in a base link that is fixed to the world - std::size_t linkID = model->body->getUserIndex(); - bool isFixedBaseLink = model->body->hasFixedBase() && - (std::size_t(_linkID) == linkID); - if (isStatic || isFixedBaseLink) + // 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 + std::size_t linkID = model->body->getUserIndex(); + isFixed = std::size_t(_linkID) == linkID; + // 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(), From 8841d701c452030f1efd712e2353d1b675642cd6 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 Apr 2024 18:38:58 +0000 Subject: [PATCH 3/3] update test Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 4 ++-- test/common_test/collisions.cc | 19 +++++++------------ 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index e50c8564b..832839b84 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1213,8 +1213,8 @@ bool SDFFeatures::AddSdfCollision( if (model->body->hasFixedBase()) { // check if it's a base link - std::size_t linkID = model->body->getUserIndex(); - isFixed = std::size_t(_linkID) == linkID; + isFixed = std::size_t(_linkID) == + static_cast(model->body->getUserIndex()); // check if link has zero dofs if (!isFixed && linkInfo->indexInModel.has_value()) { diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc index 9c16cd3ef..db2567e41 100644 --- a/test/common_test/collisions.cc +++ b/test/common_test/collisions.cc @@ -147,25 +147,20 @@ TYPED_TEST(CollisionTest, MeshAndPlane) } } -struct CollisionStaticFeaturesList : gz::physics::FeatureList< +using CollisionStaticFeaturesList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfWorld, gz::physics::GetContactsFromLastStepFeature, gz::physics::ForwardStep -> { }; +>; -template -class CollisionStaticTest : - public CollisionTest{}; -using CollisionStaticTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(CollisionStaticTest, - CollisionStaticTestTypes); - -TYPED_TEST(CollisionStaticTest, StaticCollisions) +using CollisionStaticTestFeaturesList = + CollisionTest; + +TEST_F(CollisionStaticTestFeaturesList, StaticCollisions) { auto getBoxStaticStr = [](const std::string &_name, - const gz::math::Pose3d &_pose) + const gz::math::Pose3d &_pose) { std::stringstream modelStaticStr; modelStaticStr << R"(