Support multiple collisions per link in bullet-featherstone

bullet-featherstone/src/SDFFeatures.cc

@@ -846,6 +846,29 @@ bool SDFFeatures::AddSdfCollision(
   Eigen::Isometry3d linkFrameToCollision;
   if (shape != nullptr)
   {
+    {
+      gz::math::Pose3d gzLinkToCollision;
+      const auto errors =
+        _collision.SemanticPose().Resolve(gzLinkToCollision, linkInfo->name);
+      if (!errors.empty())
+      {
+        gzerr << "An error occurred while resolving the transform of the "
+          << "collider [" << _collision.Name() << "] in Link ["
+          << linkInfo->name << "] in Model [" << model->name << "]:\n";
+        for (const auto &error : errors)
+        {
+          gzerr << error << "\n";
+        }
+
+        return false;
+      }
+
+      linkFrameToCollision = gz::math::eigen3::convert(gzLinkToCollision);
+    }
+
+    const btTransform btInertialToCollision =
+      convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision);
+
     int linkIndexInModel = -1;
     if (linkInfo->indexInModel.has_value())
       linkIndexInModel = *linkInfo->indexInModel;
@@ -859,29 +882,6 @@ bool SDFFeatures::AddSdfCollision(
       auto collider = std::make_unique<btMultiBodyLinkCollider>(
         model->body.get(), linkIndexInModel);
 
-      {
-        gz::math::Pose3d gzLinkToCollision;
-        const auto errors =
-          _collision.SemanticPose().Resolve(gzLinkToCollision, linkInfo->name);
-        if (!errors.empty())
-        {
-          gzerr << "An error occurred while resolving the transform of the "
-                << "collider [" << _collision.Name() << "] in Link ["
-                << linkInfo->name << "] in Model [" << model->name << "]:\n";
-          for (const auto &error : errors)
-          {
-            gzerr << error << "\n";
-          }
-
-          return false;
-        }
-
-        linkFrameToCollision = gz::math::eigen3::convert(gzLinkToCollision);
-      }
-
-      const btTransform btInertialToCollision =
-        convertTf(linkInfo->inertiaToLinkFrame * linkFrameToCollision);
-
       linkInfo->shape->addChildShape(btInertialToCollision, shape.get());
 
       collider->setCollisionShape(linkInfo->shape.get());
@@ -928,6 +928,10 @@ bool SDFFeatures::AddSdfCollision(
           btBroadphaseProxy::AllFilter);
       }
     }
+    else if (linkInfo->shape)
+    {
+      linkInfo->shape->addChildShape(btInertialToCollision, shape.get());
+    }
     else
     {
       // TODO(MXG): Maybe we should check if the new collider's properties

test/common_test/simulation_features.cc

@@ -1090,6 +1090,48 @@ TYPED_TEST(SimulationFeaturesTestFeaturesContactPropertiesCallback, ContactPrope
   }
 }
 
+TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions)
+{
+  for (const std::string &name : this->pluginNames)
+  {
+    if(this->PhysicsEngineName(name) == "tpe")
+      GTEST_SKIP();
+
+    auto worlds = LoadWorlds<Features>(
+      this->loader,
+      this->pluginNames,
+      gz::common::joinPaths(TEST_WORLD_DIR, "multiple_collisions.sdf"));
+
+    for (const auto &world : worlds)
+    {
+      // model free group test
+      auto model = world->GetModel("box");
+      auto freeGroup = model->FindFreeGroup();
+      ASSERT_NE(nullptr, freeGroup);
+      GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
+      ASSERT_NE(nullptr, freeGroup->CanonicalLink());
+      GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION
+      ASSERT_NE(nullptr, freeGroup->RootLink());
+
+      auto link = model->GetLink("box_link");
+      auto freeGroupLink = link->FindFreeGroup();
+      ASSERT_NE(nullptr, freeGroupLink);
+
+      StepWorld<Features>(world, true);
+
+      auto frameData = model->GetLink(0)->FrameDataRelativeToWorld();
+      EXPECT_EQ(gz::math::Pose3d(0, 0, 4, 0, 0, 0),
+                gz::math::eigen3::convert(frameData.pose));
+
+      StepWorld<Features>(world, false, 1000);
+      frameData = model->GetLink(0)->FrameDataRelativeToWorld();
+      gz::math::Pose3d framePose = gz::math::eigen3::convert(frameData.pose);
+
+      EXPECT_NEAR(0.5, framePose.Z(), 0.1);
+    }
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);

test/common_test/worlds/multiple_collisions.sdf

@@ -0,0 +1,94 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+
+            <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+        <engine><filename>gz-physics-bullet-featherstone-plugin</filename></engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <emissive>0.8 0.8 0.8 1</emissive>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="box">
+      <static>false</static>
+      <pose>0 0 4.0 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision_upper">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="box_visual_upper">
+          <pose>0 0 0.25 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </visual>
+
+
+        <collision name="box_collision_lower">
+          <pose>0 0 -0.25 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="box_visual_lower">
+          <pose>0 0 -0.25 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>