[bullet-featherstone] Ignore collision between static objects and objects with world joint

bullet-featherstone/src/SDFFeatures.cc

@@ -1205,7 +1205,24 @@ bool SDFFeatures::AddSdfCollision(
 
       auto *world = this->ReferenceInterface<WorldInfo>(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<std::size_t>(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(),

test/common_test/collisions.cc

@@ -16,28 +16,34 @@
 */
 #include <gtest/gtest.h>
 
+#include <sstream>
 #include <string>
 
 #include <gz/common/Console.hh>
 #include <gz/plugin/Loader.hh>
 
 #include "test/Resources.hh"
 #include "test/TestLibLoader.hh"
+#include "Worlds.hh"
 
 #include <gz/physics/FindFeatures.hh>
 #include <gz/physics/RequestEngine.hh>
 #include <gz/physics/ConstructEmpty.hh>
 #include <gz/physics/ForwardStep.hh>
 #include <gz/physics/FrameSemantics.hh>
 #include <gz/physics/FreeJoint.hh>
+#include <gz/physics/GetContacts.hh>
 #include <gz/physics/GetEntities.hh>
 #include <gz/physics/mesh/MeshShape.hh>
 #include <gz/physics/PlaneShape.hh>
 #include <gz/physics/FixedJoint.hh>
+#include <gz/physics/sdf/ConstructModel.hh>
 #include <gz/physics/sdf/ConstructWorld.hh>
 
 #include <gz/common/MeshManager.hh>
 
+#include <sdf/Root.hh>
+
 template <class T>
 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<CollisionStaticFeaturesList>;
+
+TEST_F(CollisionStaticTestFeaturesList, StaticCollisions)
+{
+  auto getBoxStaticStr = [](const std::string &_name,
+                            const gz::math::Pose3d &_pose)
+  {
+    std::stringstream modelStaticStr;
+    modelStaticStr << R"(
+    <sdf version="1.11">
+      <model name=")";
+    modelStaticStr << _name << R"(">
+        <pose>)";
+    modelStaticStr << _pose;
+    modelStaticStr << R"(</pose>
+        <link name="body">
+          <collision name="collision">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <static>true</static>
+      </model>
+    </sdf>)";
+    return modelStaticStr.str();
+  };
+
+  auto getBoxFixedJointStr = [](const std::string &_name,
+                                const gz::math::Pose3d &_pose)
+  {
+    std::stringstream modelFixedJointStr;
+    modelFixedJointStr << R"(
+    <sdf version="1.11">
+      <model name=")";
+    modelFixedJointStr << _name << R"(">
+        <pose>)";
+    modelFixedJointStr << _pose;
+    modelFixedJointStr << R"(</pose>
+        <link name="body">
+          <collision name="collision">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <joint name="world_fixed" type="fixed">
+          <parent>world</parent>
+          <child>body</child>
+        </joint>
+      </model>
+    </sdf>)";
+    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<CollisionStaticFeaturesList>::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);