Repository: gazebosim/gz-physics
Pull request #727: Fix collision checking issues in bullet-featherstone
Author: login: shameekganguly
Labels: name: Bullet
description: Bullet engine
name: 🏛️ ionic
description: Gazebo Ionic
name: 🪵 jetty
description: Gazebo Jetty
Created: 2025-04-16T20:14:48Z | Merged: 2025-04-17T23:08:41Z

# 🦟 Bug fix

Fixes three issues in bullet-featherstone plugin:
1. After calling `SetPosition` to set a joint position, incorrect collision transforms are used in the next forward step
2. After calling `SetWorldPose` to set the pose of a FreeGroup, incorrect collision transforms are used in the next forward step
3. Collisions are ignored between a static object and any link on a fixed-base model that is a child of a fixed joint

## Summary
Added a `GzMultiBody` wrapper for `btMultiBody` to ensure that a flag is set whenever joint position is set or base transform is set indicating that collision transforms need to be updated. Then collision transforms are updated in the next forward step for any body which has the flag set, and the flag is reset.

For bug 3 above, I updated `SDFFeatures::CreateLinkCollider` to check the total dofs for a link on a fixed-base model, counting from the base, rather than just the dofs for the parent joint.

## Checklist
- [x] Signed all commits for DCO
- [x] Added tests
- [ ] Updated documentation (as needed)
- [ ] Updated migration guide (as needed)
- [ ] Consider updating Python bindings (if the library has them)
- [ ] `codecheck` passed (See [contributing](https://gazebosim.org/docs/all/contributing#contributing-code))
- [ ] All tests passed (See [test coverage](https://gazebosim.org/docs/all/contributing#test-coverage))
- [ ] While waiting for a review on your PR, please help review [another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Agazebosim+archived%3Afalse+) to support the maintainers

**Note to maintainers**: Remember to use **Squash-Merge** and edit the commit message to match the pull request summary while retaining `Signed-off-by` messages.



Diff:
diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc
index ac66f5746..76af9fe58 100644
--- a/bullet-featherstone/src/Base.cc
+++ b/bullet-featherstone/src/Base.cc
@@ -17,6 +17,9 @@
 
 #include "Base.hh"
 
+#include <LinearMath/btVector3.h>
+#include <LinearMath/btQuaternion.h>
+
 #include <utility>
 
 namespace gz {
@@ -56,6 +59,44 @@ WorldInfo::WorldInfo(std::string name_)
   this->world->getSolverInfo().m_numIterations = 50u;
 }
 
+void GzMultiBody::SetJointPosForDof(
+  int _jointIndex,
+  std::size_t _dof,
+  btScalar _value)
+{
+  btScalar *positions =
+      this->getJointPosMultiDof(_jointIndex);
+  positions[_dof] = static_cast<btScalar>(_value);
+
+  // Call setJointPosMultiDof to ensure that the link pose cache is updated.
+  this->setJointPosMultiDof(_jointIndex, positions);
+
+  this->needsCollisionTransformsUpdate = true;
+}
+
+btScalar GzMultiBody::GetJointPosForDof(int _jointIndex, std::size_t _dof) const
+{
+  return this->getJointPosMultiDof(_jointIndex)[_dof];
+}
+
+void GzMultiBody::SetBaseWorldTransform(const btTransform &_pose)
+{
+  this->setBaseWorldTransform(_pose);
+  this->needsCollisionTransformsUpdate = true;
+}
+
+void GzMultiBody::UpdateCollisionTransformsIfNeeded()
+{
+  if (this->needsCollisionTransformsUpdate)
+  {
+    btAlignedObjectArray<btQuaternion> scratchWorldToLocal;
+    btAlignedObjectArray<btVector3> scratchLocalOrigin;
+    this->updateCollisionObjectWorldTransforms(
+      scratchWorldToLocal, scratchLocalOrigin);
+    this->needsCollisionTransformsUpdate = false;
+  }
+}
+
 }  // namespace bullet_featherstone
 }  // namespace physics
 }  // namespace gz
diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh
index fcf82e70d..2995e441e 100644
--- a/bullet-featherstone/src/Base.hh
+++ b/bullet-featherstone/src/Base.hh
@@ -81,13 +81,54 @@ struct WorldInfo
   explicit WorldInfo(std::string name);
 };
 
+/// \brief Custom `GzMultiBody` wrapper for `btMultiBody` to ensure that a flag
+/// is set whenever joint position is set or base transform is set indicating
+/// that collision transforms need to be updated.
+class GzMultiBody: public btMultiBody
+{
+  using btMultiBody::btMultiBody;
+
+  /// \brief Set position for a particular joint dof and set a flag that the
+  /// collision world transforms need to be updated before performing collision
+  /// queries. Use this method to set a joint dof position instead of the base
+  /// class methods `setJointPos`/ `setJointPosMultiDof`/
+  /// (non-const)`getJointPosMultiDof` to ensure that collision queries after
+  /// setting joint positions are accurate.
+  public: void SetJointPosForDof(
+    int _jointIndex,
+    std::size_t _dof,
+    btScalar _value);
+  private: using btMultiBody::setJointPos;
+  private: using btMultiBody::setJointPosMultiDof;
+  private: using btMultiBody::getJointPosMultiDof;
+
+  /// \brief Get the position of a particular joint dof.
+  /// This method is needed because the private using-declaration above hides
+  /// both the const and non-const overloads of
+  /// `btMultiBody::getJointPosMultiDof`.
+  public: btScalar GetJointPosForDof(int _jointIndex, std::size_t _dof) const;
+
+  /// \brief Set base transform in world.
+  /// Use this method to set the transform for a moving base model instead of
+  /// the base class method `setBaseWorldTransform` to ensure that collision
+  /// queries after setting the transform are accurate.
+  public: void SetBaseWorldTransform(const btTransform &_pose);
+  private: using btMultiBody::setBaseWorldTransform;
+
+  /// \brief Update collision transforms if `needsCollisionTransformsUpdate` is
+  /// set and reset the flag.
+  public: void UpdateCollisionTransformsIfNeeded();
+
+  private: bool needsCollisionTransformsUpdate = false;
+};
+
 struct ModelInfo
 {
   std::string name;
   Identity world;
   int indexInWorld;
   Eigen::Isometry3d baseInertiaToLinkFrame;
-  std::shared_ptr<btMultiBody> body;
+  std::shared_ptr<GzMultiBody> body;
 
   std::vector<std::size_t> linkEntityIds;
   std::vector<std::size_t> jointEntityIds;
@@ -104,7 +145,7 @@ struct ModelInfo
     std::string _name,
     Identity _world,
     Eigen::Isometry3d _baseInertiaToLinkFrame,
-    std::shared_ptr<btMultiBody> _body)
+    std::shared_ptr<GzMultiBody> _body)
     : name(std::move(_name)),
       world(std::move(_world)),
       baseInertiaToLinkFrame(_baseInertiaToLinkFrame),
@@ -324,7 +365,7 @@ class Base : public Implements3d<FeatureList<Feature>>
     std::string _name,
     Identity _worldID,
     Eigen::Isometry3d _baseInertialToLinkFrame,
-    std::shared_ptr<btMultiBody> _body)
+    std::shared_ptr<GzMultiBody> _body)
   {
     const auto id = this->GetNextEntity();
     auto model = std::make_shared<ModelInfo>(
@@ -349,7 +390,7 @@ class Base : public Implements3d<FeatureList<Feature>>
     Identity _parentID,
     Identity _worldID,
     Eigen::Isometry3d _baseInertialToLinkFrame,
-    std::shared_ptr<btMultiBody> _body)
+    std::shared_ptr<GzMultiBody> _body)
   {
     const auto id = this->GetNextEntity();
     auto model = std::make_shared<ModelInfo>(
diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
index 6cf0918b5..6613ff343 100644
--- a/bullet-featherstone/src/FreeGroupFeatures.cc
+++ b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -167,8 +167,8 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   if (model)
   {
-    model->body->setBaseWorldTransform(
-        convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));
+    model->body->SetBaseWorldTransform(
+      convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));
 
     model->body->wakeUp();
 
diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
index c133b6639..18ee1100a 100644
--- a/bullet-featherstone/src/JointFeatures.cc
+++ b/bullet-featherstone/src/JointFeatures.cc
@@ -141,7 +141,7 @@ double JointFeatures::GetJointPosition(
   if (identifier)
   {
     const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
-    return model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof];
+    return model->body->GetJointPosForDof(identifier->indexInBtModel, _dof);
   }
 
   // The base joint never really has a position. It is either a Free Joint or
@@ -256,8 +256,7 @@ void JointFeatures::SetJointPosition(
     return;
 
   const auto *model = this->ReferenceInterface<ModelInfo>(joint->model);
-  model->body->getJointPosMultiDof(identifier->indexInBtModel)[_dof] =
-      static_cast<btScalar>(_value);
+  model->body->SetJointPosForDof(identifier->indexInBtModel, _dof, _value);
   model->body->wakeUp();
 }
 
diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index ebb7c686c..e1e8b546a 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -500,7 +500,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
 
   std::unordered_map<const ::sdf::Model*, Identity> modelIDs;
   std::size_t rootModelID = 0u;
-  std::shared_ptr<btMultiBody> rootMultiBody;
+  std::shared_ptr<GzMultiBody> rootMultiBody;
   // Add all  models, including nested models
   auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model,
                        auto &&_addModels)
@@ -525,7 +525,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
         {
           auto worldIdentity = this->GenerateIdentity(
               _modelOrWorldID, worldIt->second);
-          rootMultiBody = std::make_shared<btMultiBody>(
+          rootMultiBody = std::make_shared<GzMultiBody>(
                 static_cast<int>(structure.flatLinks.size()),
                 structure.mass,
                 structure.inertia,
@@ -872,7 +872,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
     *worldToModel * modelToNestedModel * *modelToRootLink *
     rootInertialToLink.inverse();
 
-  model->body->setBaseWorldTransform(convertTf(worldToRootCom));
+  model->body->SetBaseWorldTransform(convertTf(worldToRootCom));
   model->body->setBaseVel(btVector3(0, 0, 0));
   model->body->setBaseOmega(btVector3(0, 0, 0));
 
@@ -1484,11 +1484,12 @@ void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic,
     // check if it's a base link
     isFixed = std::size_t(_linkID) ==
         static_cast<std::size_t>(modelInfo->body->getUserIndex());
-    // check if link has zero dofs
+    // check if link has zero dofs from model base.
     if (!isFixed && linkInfo->indexInModel.has_value())
     {
-       isFixed = modelInfo->body->getLink(
-          linkInfo->indexInModel.value()).m_dofCount == 0;
+      auto link = modelInfo->body->getLink(linkInfo->indexInModel.value());
+      int totalLinkDofs = link.m_dofOffset + link.m_dofCount;
+      isFixed = totalLinkDofs == 0;
     }
   }
   if (_isStatic || isFixed)
diff --git a/bullet-featherstone/src/SimulationFeatures.cc b/bullet-featherstone/src/SimulationFeatures.cc
index e74cdbebc..10afd870e 100644
--- a/bullet-featherstone/src/SimulationFeatures.cc
+++ b/bullet-featherstone/src/SimulationFeatures.cc
@@ -44,6 +44,17 @@ void SimulationFeatures::WorldForwardStep(
     stepSize = dt.count();
   }
 
+  // Bullet updates collision transforms *after* forward integration. But in
+  // some case (e.g. if joint positions were updated), collision transforms may
+  // need to be manually updated before stepping the Bullet simulation.
+  for (auto & model : this->models)
+  {
+    if (model.second->body)
+    {
+      model.second->body->UpdateCollisionTransformsIfNeeded();
+    }
+  }
+
   // \todo(iche033) Stepping sim with varying dt may not work properly.
   // One example is the motor constraint that's created in
   // JointFeatures::SetJointVelocityCommand which assumes a fixed step
diff --git a/test/common_test/collisions.cc b/test/common_test/collisions.cc
index 647bffa39..d1f7ec8cb 100644
--- a/test/common_test/collisions.cc
+++ b/test/common_test/collisions.cc
@@ -455,6 +455,9 @@ TEST_F(CollisionStaticTestFeaturesList, StaticCollisions)
 
   for (const std::string &name : this->pluginNames)
   {
+    // TPE does not support collision checking with plane shapes.
+    if (this->PhysicsEngineName(name) == "tpe") continue;
+
     std::cout << "Testing plugin: " << name << std::endl;
     gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
 
@@ -509,6 +512,104 @@ TEST_F(CollisionStaticTestFeaturesList, StaticCollisions)
   }
 }
 
+TEST_F(CollisionStaticTestFeaturesList, StaticCollisionsWithFixedBaseMovingLink)
+{
+  auto getBoxStr = [](const std::string &_name,
+    const gz::math::Pose3d &_pose)
+  {
+    std::stringstream modelStr;
+    modelStr << R"(
+    <sdf version="1.11">
+      <model name=")";
+    modelStr << _name << R"(">
+        <pose>)";
+    modelStr << _pose;
+    modelStr << R"(</pose>
+        <link name="body" />
+        <joint name="world_fixed" type="fixed">
+          <parent>world</parent>
+          <child>body</child>
+        </joint>
+        <link name="moving1">
+          <collision name="collision">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <joint name="slider" type="prismatic">
+          <parent>body</parent>
+          <child>moving1</child>
+          <axis>
+            <xyz>0 0 1</xyz>
+          </axis>
+        </joint>
+        <link name="moving2">
+          <pose>2 0 0 0 0 0</pose>
+          <collision name="collision">
+            <geometry>
+              <box><size>1 1 1</size></box>
+            </geometry>
+          </collision>
+        </link>
+        <joint name="moving1_fixed" type="fixed">
+          <parent>moving1</parent>
+          <child>moving2</child>
+        </joint>
+      </model>
+    </sdf>)";
+    return modelStr.str();
+  };
+
+  for (const std::string &name : this->pluginNames)
+  {
+    // TPE does not support collision checking with plane shapes.
+    if (this->PhysicsEngineName(name) == "tpe") continue;
+
+    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(getBoxStr(
+        "box_fixed_base_moving_links", 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;
+    world->Step(output, state, input);
+
+    // box_fixed_base_moving_links overlaps with ground plane on both moving1
+    // and moving2 links, verify that contacts are present on both links.
+    auto contacts = world->GetContactsFromLastStep();
+    EXPECT_LE(2u, contacts.size());
+
+    std::unordered_set<std::size_t> collisionIds;
+    using WorldShapeType = gz::physics::World<
+        gz::physics::FeaturePolicy3d, CollisionStaticFeaturesList>;
+    for (const auto &contact: contacts) {
+      const auto &contactPoint = contact.Get<WorldShapeType::ContactPoint>();
+      collisionIds.insert(contactPoint.collision1->EntityID());
+      collisionIds.insert(contactPoint.collision2->EntityID());
+    }
+    EXPECT_EQ(3u, collisionIds.size());
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc
index fb92c4f7b..ed5c5a61b 100644
--- a/test/common_test/joint_features.cc
+++ b/test/common_test/joint_features.cc
@@ -39,6 +39,7 @@
 #include <gz/physics/ForwardStep.hh>
 #include <gz/physics/FreeGroup.hh>
 #include <gz/physics/FreeJoint.hh>
+#include <gz/physics/GetContacts.hh>
 #include <gz/physics/GetEntities.hh>
 #include <gz/physics/Joint.hh>
 #include <gz/physics/RemoveEntities.hh>
@@ -86,15 +87,18 @@ class JointFeaturesTest:
 };
 
 struct JointFeatureList : gz::physics::FeatureList<
+    gz::physics::FindFreeGroupFeature,
     gz::physics::ForwardStep,
     gz::physics::GetBasicJointProperties,
     gz::physics::GetBasicJointState,
+    gz::physics::GetContactsFromLastStepFeature,
     gz::physics::GetEngineInfo,
     gz::physics::GetJointFromModel,
     gz::physics::GetLinkFromModel,
     gz::physics::GetModelFromWorld,
     gz::physics::LinkFrameSemantics,
     gz::physics::SetBasicJointState,
+    gz::physics::SetFreeGroupWorldPose,
     gz::physics::SetJointVelocityCommandFeature,
     gz::physics::sdf::ConstructSdfWorld
 > { };
@@ -205,6 +209,72 @@ TYPED_TEST(JointFeaturesTest, JointSetCommand)
   }
 }
 
+TYPED_TEST(JointFeaturesTest, JointSetPositionWithContact)
+{
+  for (const std::string &name : this->pluginNames)
+  {
+    std::cout << "Testing plugin: " << name << std::endl;
+    gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
+
+    auto engine = gz::physics::RequestEngine3d<JointFeatureList>::From(plugin);
+    ASSERT_NE(nullptr, engine);
+
+    sdf::Root root;
+    const sdf::Errors errors = root.Load(
+        common_test::worlds::kPendulumJointWrenchSdf);
+    ASSERT_TRUE(errors.empty()) << errors.front();
+
+    auto world = engine->ConstructWorld(*root.WorldByIndex(0));
+    ASSERT_NE(nullptr, world);
+
+    auto model = world->GetModel("pendulum");
+    ASSERT_NE(nullptr, model);
+    auto motorJoint = model->GetJoint("motor_joint");
+    ASSERT_NE(nullptr, motorJoint);
+
+    gz::physics::ForwardStep::Output output;
+    gz::physics::ForwardStep::State state;
+    gz::physics::ForwardStep::Input input;
+
+    world->Step(output, state, input);
+    auto contacts = world->GetContactsFromLastStep();
+    const std::size_t numInitialContacts = contacts.size();
+
+    // Place box such that it is in collision with the pendulum arm at joint
+    // position 0.
+    auto box = world->GetModel("box");
+    ASSERT_NE(nullptr, box);
+    auto boxFreeGroup = box->FindFreeGroup();
+    ASSERT_NE(nullptr, boxFreeGroup);
+    gz::physics::Pose3d X_WB(Eigen::Translation3d(0.5, 0, 0.65));
+    boxFreeGroup->SetWorldPose(X_WB);
+
+    world->Step(output, state, input);
+    contacts = world->GetContactsFromLastStep();
+    EXPECT_LT(numInitialContacts, contacts.size());
+
+    // Move pendulum away from box.
+    motorJoint->SetPosition(0, GZ_DTOR(90.0));
+
+    world->Step(output, state, input);
+    contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(numInitialContacts, contacts.size());
+
+    // Step until pendulum falls and rests again on the box.
+    for (int i = 0; i < 1000; ++i)
+    {
+      world->Step(output, state, input);
+    }
+
+    // Sanity check that the pendulum is at rest. A small non-zero threshold is
+    // set to accommodate small joint velocity due to error reduction.
+    EXPECT_NEAR(0.0, motorJoint->GetVelocity(0), 2e-3);
+
+    contacts = world->GetContactsFromLastStep();
+    EXPECT_LT(numInitialContacts, contacts.size());
+  }
+}
+
 struct JointFeaturePositionLimitsList : gz::physics::FeatureList<
     gz::physics::ForwardStep,
     gz::physics::GetBasicJointProperties,


Reviews:
user: login: iche033
body: looks good to me, just minor comments
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-17T18:16:02Z
user: login: iche033
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-17T18:16:14Z
user: login: shameekganguly
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-17T20:18:42Z
user: login: shameekganguly
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-17T20:19:29Z
user: login: shameekganguly
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-17T20:20:20Z
user: login: iche033
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-04-17T21:11:19Z
user: login: scpeters
state: APPROVED
author_association: MEMBER
submitted_at: 2025-04-17T22:13:10Z
user: login: scpeters
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-17T22:13:42Z

Review comments:
diff_hunk: @@ -171,12 +176,14 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
     gz::physics::ForwardStep::State state;
     gz::physics::ForwardStep::Output output;
 
+    std::cout << "Here6 " << name << std::endl;
     const size_t numSteps = 1000;
     for (size_t i = 0; i < numSteps; ++i)
     {
       world->Step(output, state, input);
     }
 
+    std::cout << "Here7 " << name << std::endl;
path: test/common_test/world_features.cc
user: login: scpeters
body: please revert the debugging messages in this file
created_at: 2025-04-16T20:45:50Z
author_association: MEMBER
diff_hunk: @@ -95,12 +95,14 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
     EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) !=
                 std::string::npos);
 
+    std::cout << "Here1 " << name << std::endl;
path: test/common_test/world_features.cc
user: login: iche033
body: leftover debug statements? 
created_at: 2025-04-17T17:50:54Z
author_association: CONTRIBUTOR
diff_hunk: @@ -56,6 +59,43 @@ WorldInfo::WorldInfo(std::string name_)
   this->world->getSolverInfo().m_numIterations = 50u;
 }
 
+void GzMultiBody::SetJointPosForDof(
+  int _jointIndex,
+  std::size_t _dof,
+  btScalar _value)
+{
+  btScalar *positions =
+      this->getJointPosMultiDof(_jointIndex);
+  positions[_dof] = static_cast<btScalar>(_value);
+
+  // Call setJointPosMultiDof to ensure that the link pose cache is updated.
+  this->setJointPosMultiDof(_jointIndex, positions);
+
+  this->needsCollisionTransformsUpdate = true;
+}
+
+btScalar GzMultiBody::GetJointPosForDof(int _jointIndex, std::size_t _dof) const
+{
+  return this->getJointPosMultiDof(_jointIndex)[_dof];
+}
+
+void GzMultiBody::SetBaseWorldTransform(const btTransform &_pose)
+{
+  this->setBaseWorldTransform(_pose);
+  this->needsCollisionTransformsUpdate = true;
+}
+
+void GzMultiBody::UpdateCollisionTransformsIfNeeded()
+{
+  if (this->needsCollisionTransformsUpdate) {
path: bullet-featherstone/src/Base.cc
user: login: iche033
body: nit: `{` on new line
```suggestion
  if (this->needsCollisionTransformsUpdate)
  {
```
created_at: 2025-04-17T17:52:33Z
author_association: CONTRIBUTOR
diff_hunk: @@ -81,13 +81,50 @@ struct WorldInfo
   explicit WorldInfo(std::string name);
 };
 
+class GzMultiBody: public btMultiBody {
path: bullet-featherstone/src/Base.hh
user: login: iche033
body: nit: 

```suggestion
class GzMultiBody: public btMultiBody
{
```
created_at: 2025-04-17T17:53:28Z
author_association: CONTRIBUTOR
diff_hunk: @@ -81,13 +81,50 @@ struct WorldInfo
   explicit WorldInfo(std::string name);
 };
 
+class GzMultiBody: public btMultiBody {
path: bullet-featherstone/src/Base.hh
user: login: iche033
body: can you also add a brief comment to mention why the `GzMultiBody` class is needed? Basically what you wrote in the description of the PR
created_at: 2025-04-17T18:09:29Z
author_association: CONTRIBUTOR
diff_hunk: @@ -95,12 +95,14 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
     EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) !=
                 std::string::npos);
 
+    std::cout << "Here1 " << name << std::endl;
path: test/common_test/world_features.cc
user: login: iche033
body: there are a few others in this file
created_at: 2025-04-17T18:16:14Z
author_association: CONTRIBUTOR
diff_hunk: @@ -95,12 +95,14 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
     EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) !=
                 std::string::npos);
 
+    std::cout << "Here1 " << name << std::endl;
path: test/common_test/world_features.cc
user: login: shameekganguly
body: Thanks for catching! Fixed in [91007902](https://github.com/gazebosim/gz-physics/pull/727/commits/91007902a9d8b2e2f7db63b80d4d546f149cd5b4)
created_at: 2025-04-17T20:18:42Z
author_association: CONTRIBUTOR
diff_hunk: @@ -56,6 +59,43 @@ WorldInfo::WorldInfo(std::string name_)
   this->world->getSolverInfo().m_numIterations = 50u;
 }
 
+void GzMultiBody::SetJointPosForDof(
+  int _jointIndex,
+  std::size_t _dof,
+  btScalar _value)
+{
+  btScalar *positions =
+      this->getJointPosMultiDof(_jointIndex);
+  positions[_dof] = static_cast<btScalar>(_value);
+
+  // Call setJointPosMultiDof to ensure that the link pose cache is updated.
+  this->setJointPosMultiDof(_jointIndex, positions);
+
+  this->needsCollisionTransformsUpdate = true;
+}
+
+btScalar GzMultiBody::GetJointPosForDof(int _jointIndex, std::size_t _dof) const
+{
+  return this->getJointPosMultiDof(_jointIndex)[_dof];
+}
+
+void GzMultiBody::SetBaseWorldTransform(const btTransform &_pose)
+{
+  this->setBaseWorldTransform(_pose);
+  this->needsCollisionTransformsUpdate = true;
+}
+
+void GzMultiBody::UpdateCollisionTransformsIfNeeded()
+{
+  if (this->needsCollisionTransformsUpdate) {
path: bullet-featherstone/src/Base.cc
user: login: shameekganguly
body: Done [0d06d119](https://github.com/gazebosim/gz-physics/pull/727/commits/0d06d11981beb26d50dbf58c737170b3f8af9d4e)
created_at: 2025-04-17T20:19:28Z
author_association: CONTRIBUTOR
diff_hunk: @@ -81,13 +81,50 @@ struct WorldInfo
   explicit WorldInfo(std::string name);
 };
 
+class GzMultiBody: public btMultiBody {
path: bullet-featherstone/src/Base.hh
user: login: shameekganguly
body: Done and done [0d06d119](https://github.com/gazebosim/gz-physics/pull/727/commits/0d06d11981beb26d50dbf58c737170b3f8af9d4e)
created_at: 2025-04-17T20:20:20Z
author_association: CONTRIBUTOR
diff_hunk: @@ -171,12 +176,14 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
     gz::physics::ForwardStep::State state;
     gz::physics::ForwardStep::Output output;
 
+    std::cout << "Here6 " << name << std::endl;
     const size_t numSteps = 1000;
     for (size_t i = 0; i < numSteps; ++i)
     {
       world->Step(output, state, input);
     }
 
+    std::cout << "Here7 " << name << std::endl;
path: test/common_test/world_features.cc
user: login: scpeters
body: this was an old comment; you've already addressed it
created_at: 2025-04-17T22:13:42Z
author_association: MEMBER

PR comments:
user: login: iche033
created_at: 2025-04-17T21:13:16Z
body: the homebrew warning is preexisting, I see it in recent homebrew builds as well, e.g. https://github.com/gazebosim/gz-physics/pull/723#issuecomment-2779801996
author_association: CONTRIBUTOR
user: login: scpeters
created_at: 2025-04-17T22:13:01Z
body: > the homebrew warning is preexisting, I see it in recent homebrew builds as well, e.g. [#723 (comment)](https://github.com/gazebosim/gz-physics/pull/723#issuecomment-2779801996)

it's coming from octomap, definitely unrelated
author_association: MEMBER
user: login: scpeters
created_at: 2025-04-17T22:29:48Z
body: > > the homebrew warning is preexisting, I see it in recent homebrew builds as well, e.g. [#723 (comment)](https://github.com/gazebosim/gz-physics/pull/723#issuecomment-2779801996)
> 
> it's coming from octomap, definitely unrelated

suppressing it in https://github.com/gazebosim/gz-physics/pull/728
author_association: MEMBER
user: login: iche033
created_at: 2025-04-17T23:08:57Z
body: @mergifyio backport gz-physics7
author_association: CONTRIBUTOR
user: login: mergify[bot]
created_at: 2025-04-17T23:09:14Z
body: > backport gz-physics7

#### ✅ Backports have been created

<details>

* [#729 Fix collision checking issues in bullet-featherstone (backport #727)](https://github.com/gazebosim/gz-physics/pull/729) has been created for branch `gz-physics7`

</details>


<!---
DO NOT EDIT
-*- Mergify Payload -*-
{"command": "backport gz-physics7", "conclusion": "success", "action_is_running": true}
-*- Mergify Payload End -*-
-->
author_association: CONTRIBUTOR