bullet-featherstone: Fix get contacts (#838)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
(cherry picked from commit 00e0fdd)

Author: 2 people

bullet-featherstone/src/SDFFeatures.cc

@@ -1351,12 +1351,18 @@ bool SDFFeatures::AddSdfCollision(
       // match the existing collider and issue a warning if they don't.
     }
 
-    this->AddCollision(
+    btCollisionShape *shapePtr = shape.get();
+    auto colID = this->AddCollision(
       CollisionInfo{
         _collision.Name(),
         std::move(shape),
         _linkID,
         linkFrameToCollision});
+
+    // Use user index to store the collision id in gz-physics
+    // This is used by GetContactsFromLastStep to retrieve the collision id
+    // from btCollisionShape
+    shapePtr->setUserIndex(std::size_t(colID));
   }
 
   return true;

bullet-featherstone/src/SimulationFeatures.cc

@@ -27,6 +27,77 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/////////////////////////////////////////////////
+bool hasConvexHullChildShapes(
+    const btCollisionShape *_shape)
+{
+  if (!_shape || !_shape->isCompound())
+    return false;
+
+  const btCompoundShape *compoundShape =
+      static_cast<const btCompoundShape *>(_shape);
+  return (compoundShape->getNumChildShapes() > 0 &&
+      compoundShape->getChildShape(0)->getShapeType() ==
+      CONVEX_HULL_SHAPE_PROXYTYPE);
+}
+
+/////////////////////////////////////////////////
+const btCollisionShape *findCollisionShape(
+    const btCompoundShape *_compoundShape, int _childIndex)
+{
+  // _childIndex should give us the index of the child shape within
+  // _compoundShape which represents the collision.
+  // One exception is when the collision is a convex decomposed mesh.
+  // In this case, the child shape is another btCompoundShape (nested), and
+  // _childIndex is the index of one of the decomposed convex hulls
+  // in the nested compound shape. The nested compound shape is the collision.
+  int childCount = _compoundShape->getNumChildShapes();
+  if (childCount > 0)
+  {
+    if (_childIndex >= 0 && _childIndex < childCount)
+    {
+      // todo(iche033) We do not have sufficient info to determine which
+      // child shape is the collision if the link has convex decomposed mesh
+      // collisions alongside of other collisions. See following example:
+      // parentLink -> boxShape0
+      //            -> boxShape1
+      //            -> compoundShape -> convexShape0
+      //                             -> convexShape1
+      // A _childIndex of 1 is ambiguous as it could refer to either
+      // boxShape1 or convexShape1
+      // return nullptr in this case to indicate ambiguity
+      if (childCount > 1)
+      {
+        for (int i = 0; i < childCount; ++i)
+        {
+          const btCollisionShape *shape = _compoundShape->getChildShape(i);
+          if (hasConvexHullChildShapes(shape))
+          {
+            static bool informed{false};
+            if (!informed)
+            {
+              gzwarn << "Unable to determine the collision id for a link with "
+                     << "both simple primitive and convex shape collisions."
+                     << std::endl;
+              informed = true;
+            }
+            return nullptr;
+          }
+        }
+      }
+
+      const btCollisionShape *shape =
+          _compoundShape->getChildShape(_childIndex);
+      return shape;
+    }
+    else
+    {
+      return _compoundShape->getChildShape(0);
+    }
+  }
+  return nullptr;
+}
+
 /////////////////////////////////////////////////
 void enforceFixedConstraint(
     btMultiBodyFixedConstraint *_fixedConstraint)
@@ -187,34 +258,47 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
   {
     btPersistentManifold* contactManifold =
       world->world->getDispatcher()->getManifoldByIndexInternal(i);
-    const btMultiBodyLinkCollider* obA =
+    const btMultiBodyLinkCollider* ob0 =
       dynamic_cast<const btMultiBodyLinkCollider*>(contactManifold->getBody0());
-    const btMultiBodyLinkCollider* obB =
+    const btMultiBodyLinkCollider* ob1 =
       dynamic_cast<const btMultiBodyLinkCollider*>(contactManifold->getBody1());
-    std::size_t collision1ID = std::numeric_limits<std::size_t>::max();
-    std::size_t collision2ID = std::numeric_limits<std::size_t>::max();
 
-    for (const auto & link : this->links)
-    {
-      if (obA == link.second->collider.get())
-      {
-        for (const auto &v : link.second->collisionNameToEntityId)
-        {
-          collision1ID = v.second;
-        }
-      }
-      if (obB == link.second->collider.get())
-      {
-        for (const auto &v : link.second->collisionNameToEntityId)
-        {
-          collision2ID = v.second;
-        }
-      }
-    }
+    if (!ob0 || !ob1)
+      continue;
+
+    const btCollisionShape *linkShape0 = ob0->getCollisionShape();
+    const btCollisionShape *linkShape1 = ob1->getCollisionShape();
+
+    if (!linkShape0 || !linkShape1 ||
+        !linkShape0->isCompound() || !linkShape1->isCompound())
+      continue;
+
+    const btCompoundShape *compoundShape0 =
+        static_cast<const btCompoundShape *>(linkShape0);
+    const btCompoundShape *compoundShape1 =
+        static_cast<const btCompoundShape *>(linkShape1);
+
     int numContacts = contactManifold->getNumContacts();
     for (int j = 0; j < numContacts; j++)
     {
       btManifoldPoint& pt = contactManifold->getContactPoint(j);
+
+      const btCollisionShape *colShape0 = findCollisionShape(
+          compoundShape0, pt.m_index0);
+      const btCollisionShape *colShape1 = findCollisionShape(
+          compoundShape1, pt.m_index1);
+
+      std::size_t collision0ID = std::numeric_limits<std::size_t>::max();
+      std::size_t collision1ID = std::numeric_limits<std::size_t>::max();
+      if (colShape0)
+        collision0ID = colShape0->getUserIndex();
+      else if (compoundShape0->getNumChildShapes() > 0)
+        collision0ID = compoundShape0->getChildShape(0)->getUserIndex();
+      if (colShape1)
+        collision1ID = colShape1->getUserIndex();
+      else if (compoundShape1->getNumChildShapes() > 0)
+        collision1ID = compoundShape1->getChildShape(0)->getUserIndex();
+
       CompositeData extraData;
 
       // Add normal, depth and wrench to extraData.
@@ -228,8 +312,8 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
       extraContactData.depth = pt.getDistance();
 
       outContacts.push_back(SimulationFeatures::ContactInternal {
+        this->GenerateIdentity(collision0ID, this->collisions.at(collision0ID)),
         this->GenerateIdentity(collision1ID, this->collisions.at(collision1ID)),
-        this->GenerateIdentity(collision2ID, this->collisions.at(collision2ID)),
         convert(pt.getPositionWorldOnA()), extraData});
       }
   }

test/common_test/simulation_features.cc

@@ -1306,11 +1306,32 @@ TYPED_TEST(SimulationFeaturesTestBasic, ShapeBoundingBox)
   }
 }
 
-TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks)
+using FeaturesCollisionContacts=  gz::physics::FeatureList<
+  gz::physics::sdf::ConstructSdfModel,
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::GetModelFromWorld,
+  gz::physics::GetLinkFromModel,
+  gz::physics::GetShapeFromLink,
+  gz::physics::ForwardStep,
+  gz::physics::GetContactsFromLastStepFeature
+>;
+
+using SimulationFeaturesCollisionContacts =
+    SimulationFeaturesTest<FeaturesCollisionContacts>;
+
+using FeaturesCollisionFilter =  gz::physics::FeatureList<
+  FeaturesCollisionContacts,
+  gz::physics::CollisionFilterMaskFeature
+>;
+
+using SimulationFeaturesCollisionFilter =
+    SimulationFeaturesTest<FeaturesCollisionFilter>;
+
+TEST_F(SimulationFeaturesCollisionFilter, CollideBitmasks)
 {
   for (const std::string &name : this->pluginNames)
   {
-    auto world = LoadPluginAndWorld<Features>(
+    auto world = LoadPluginAndWorld<FeaturesCollisionFilter>(
       this->loader,
       name,
       common_test::worlds::kShapesBitmaskWorld);
@@ -1319,7 +1340,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks)
     auto filteredBox = world->GetModel("box_filtered");
     auto collidingBox = world->GetModel("box_colliding");
 
-    auto checkedOutput = StepWorld<Features>(world, true).first;
+    auto checkedOutput = StepWorld<FeaturesCollisionFilter>(world, true).first;
     EXPECT_TRUE(checkedOutput);
     auto contacts = world->GetContactsFromLastStep();
     // Only box_colliding should collide with box_base
@@ -1332,7 +1353,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks)
     // Also test the getter
     EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask());
     // Step and make sure there are no collisions
-    checkedOutput = StepWorld<Features>(world, false).first;
+    checkedOutput = StepWorld<FeaturesCollisionFilter>(world, false).first;
     EXPECT_FALSE(checkedOutput);
     contacts = world->GetContactsFromLastStep();
     EXPECT_EQ(0u, contacts.size());
@@ -1341,7 +1362,7 @@ TYPED_TEST(SimulationFeaturesTestBasic, CollideBitmasks)
     // Equivalent to 0xFF
     collidingShape->RemoveCollisionFilterMask();
     filteredShape->RemoveCollisionFilterMask();
-    checkedOutput = StepWorld<Features>(world, false).first;
+    checkedOutput = StepWorld<FeaturesCollisionFilter>(world, false).first;
     EXPECT_FALSE(checkedOutput);
     // Expect box_filtered and box_colliding to collide with box_base
     contacts = world->GetContactsFromLastStep();
@@ -1530,6 +1551,101 @@ TYPED_TEST(SimulationFeaturesTestBasic, RetrieveContacts)
   }
 }
 
+TEST_F(SimulationFeaturesCollisionContacts,
+       ContactsForLinkWithMultipleCollisions)
+{
+  // This test verifies that the collision entities in contact points
+  // are correct. Thest test world consists of a box model with a single
+  // link containining multiple collisions over a ground plane. Verify
+  // that only the bottom collision in the box model should collide with
+  // the ground plane.
+
+  auto getModelStr = [](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="box_collision_mid">
+            <pose>0 0 1.0 0 0 0</pose>
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+          <collision name="box_collision_bottom">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+          <collision name="box_collision_top">
+            <pose>0 0 2.0 0 0 0</pose>
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <static>false</static>
+      </model>
+    </sdf>)";
+    return modelStaticStr.str();
+  };
+
+  for (const std::string &name : this->pluginNames)
+  {
+    // TPE does not support collision checking with plane shapes.
+    if (this->PhysicsEngineName(name) == "tpe") continue;
+
+    auto world =
+      LoadPluginAndWorld<FeaturesCollisionContacts>(
+        this->loader,
+        name,
+        common_test::worlds::kGroundSdf);
+    ASSERT_NE(nullptr, world);
+
+    sdf::Root root;
+    sdf::Errors errors = root.LoadSdfString(getModelStr(
+        "boxes", gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)));
+    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);
+    }
+
+    // box lands on ground plane
+    // verify contacts
+    auto contacts = world->GetContactsFromLastStep();
+    EXPECT_LT(0u, contacts.size());
+
+    for (auto contact : contacts)
+    {
+      const auto &contactPoint = contact.template Get<
+          gz::physics::World3d<FeaturesCollisionContacts>::ContactPoint>();
+      ASSERT_TRUE(contactPoint.collision1);
+      ASSERT_TRUE(contactPoint.collision2);
+      EXPECT_NE(contactPoint.collision1, contactPoint.collision2);
+
+      auto c1 = contactPoint.collision1;
+      auto c2 = contactPoint.collision2;
+      // Contacts should be between ground plane model's 'collision'
+      // and box model's 'box_collision_bottom' collision
+      EXPECT_TRUE(c1->GetName() == "collision" ||
+                  c1->GetName() == "box_collision_bottom");
+    }
+  }
+}
+
 struct FeaturesContactPropertiesCallback : gz::physics::FeatureList<
   gz::physics::ConstructEmptyWorldFeature,