From 84904b4c30e3bc274c8125d344d6c39fa67e42a5 Mon Sep 17 00:00:00 2001
From: Alejandro Hernandez Cordero <ahcorde@gmail.com>
Date: Wed, 4 Mar 2026 15:24:40 +0100
Subject: [PATCH 1/2] Added gravity and static state to Bullet and
 bullet-fetherstone

Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>
---
 bullet-featherstone/src/Base.hh              |   3 +
 bullet-featherstone/src/FreeGroupFeatures.cc | 172 ++++++++++++++++++-
 bullet/src/FreeGroupFeatures.cc              | 156 ++++++++++++++++-
 bullet/src/FreeGroupFeatures.hh              |  30 +++-
 test/common_test/simulation_features.cc      |   2 -
 5 files changed, 352 insertions(+), 11 deletions(-)

diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh
index d2dda2bb3..aaaa159d5 100644
--- a/bullet-featherstone/src/Base.hh
+++ b/bullet-featherstone/src/Base.hh
@@ -78,6 +78,9 @@ struct WorldInfo
 
   double stepSize = 0.001;
 
+  bool gravityEnabled = true;
+  btVector3 gravity = {0, 0, -9.8};
+
   explicit WorldInfo(std::string name);
 };
 
diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
index a24f6928e..7382ae61f 100644
--- a/bullet-featherstone/src/FreeGroupFeatures.cc
+++ b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -24,6 +24,58 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+namespace
+{
+/////////////////////////////////////////////////
+void MakeLinkColliderStatic(LinkInfo *_linkInfo)
+{
+  if (!_linkInfo || !_linkInfo->collider)
+    return;
+
+  std::cout << "MakeLinkColliderStatic: " << _linkInfo->name << " " << _linkInfo->isStaticOrFixed << std::endl;
+  if (_linkInfo->isStaticOrFixed)
+    return;
+
+  btBroadphaseProxy *proxy = _linkInfo->collider->getBroadphaseHandle();
+  if (!proxy)
+    return;
+
+  proxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
+  proxy->m_collisionFilterMask =
+      btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
+#if BT_BULLET_VERSION >= 307
+  _linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+#endif
+}
+
+/////////////////////////////////////////////////
+void MakeLinkColliderDynamic(LinkInfo *_linkInfo)
+{
+  if (!_linkInfo || !_linkInfo->collider)
+    return;
+
+  btBroadphaseProxy *proxy = _linkInfo->collider->getBroadphaseHandle();
+  if (!proxy)
+    return;
+
+  std::cout << "MakeLinkColliderDynamic: " << _linkInfo->name << " " << _linkInfo->isStaticOrFixed << std::endl;
+  if (_linkInfo->isStaticOrFixed)
+    return;
+
+  if ((proxy->m_collisionFilterGroup &
+      btBroadphaseProxy::StaticFilter) == 0)
+  {
+    return;
+  }
+
+  proxy->m_collisionFilterGroup = btBroadphaseProxy::DefaultFilter;
+  proxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
+#if BT_BULLET_VERSION >= 307
+  _linkInfo->collider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
+#endif
+}
+}  // namespace
+
 /////////////////////////////////////////////////
 btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex)
 {
@@ -163,12 +215,96 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
 void FreeGroupFeatures::SetFreeGroupGravityEnabled(
     const Identity &_groupID, bool _enabled)
 {
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (!model)
+    return;
+
+  if (model->body)
+  {
+    auto *worldInfo = this->ReferenceInterface<WorldInfo>(model->world);
+    if (worldInfo && worldInfo->world)
+    {
+      if (worldInfo->gravityEnabled)
+      {
+        worldInfo->gravity = worldInfo->world->getGravity();
+      }
+      if (_enabled)
+      {
+        worldInfo->world->setGravity(worldInfo->gravity);
+      }
+      else
+      {
+        worldInfo->world->setGravity(btVector3(0, 0, 0));
+      }
+      worldInfo->gravityEnabled = _enabled;
+    }
+    model->body->wakeUp();
+  }
+
+  for (const auto nestedModelID : model->nestedModelEntityIds)
+  {
+    auto nestedModelIt = this->models.find(nestedModelID);
+    if (nestedModelIt == this->models.end())
+      continue;
+    auto nestedIdentity = this->GenerateIdentity(
+        nestedModelID, nestedModelIt->second);
+    this->SetFreeGroupGravityEnabled(nestedIdentity, _enabled);
+  }
+}
+
+/////////////////////////////////////////////////
+bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
+    const Identity &_groupID)
+{
+  const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (!model)
+    return false;
+
+  bool gravityEnabled = false;
+  auto *worldInfo = this->ReferenceInterface<WorldInfo>(model->world);
+  if (worldInfo)
+  {
+      gravityEnabled = worldInfo->gravityEnabled;
+  }
+
+  for (const auto nestedModelID : model->nestedModelEntityIds)
+  {
+    auto nestedModelIt = this->models.find(nestedModelID);
+    if (nestedModelIt == this->models.end())
+      continue;
+    auto nestedIdentity = this->GenerateIdentity(
+        nestedModelID, nestedModelIt->second);
+    gravityEnabled = gravityEnabled &&
+        this->GetFreeGroupGravityEnabled(nestedIdentity);
+  }
+
+  return gravityEnabled;
 }
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupStaticState(
     const Identity &_groupID)
 {
+  const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (!model)
+    return false;
+
+  bool isStatic = true;
+  if (model->body)
+    isStatic = model->body->hasFixedBase();
+
+  for (const auto nestedModelID : model->nestedModelEntityIds)
+  {
+    auto nestedModelIt = this->models.find(nestedModelID);
+    if (nestedModelIt == this->models.end())
+      continue;
+    auto nestedIdentity = this->GenerateIdentity(
+        nestedModelID, nestedModelIt->second);
+    isStatic = isStatic &&
+        this->GetFreeGroupStaticState(nestedIdentity);
+  }
+
+  return isStatic;
 }
 
 /////////////////////////////////////////////////
@@ -176,12 +312,38 @@ void FreeGroupFeatures::SetFreeGroupStaticState(
     const Identity &_groupID,
     bool _enabled)
 {
-}
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (!model)
+    return;
 
-/////////////////////////////////////////////////
-bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
-    const Identity &_groupID)
-{
+  if (model->body)
+  {
+    model->body->setFixedBase(_enabled);
+    std::cout << "SetFreeGroupStaticState: " << _enabled << std::endl;
+    model->body->wakeUp();
+  }
+
+  for (const auto linkID : model->linkEntityIds)
+  {
+    auto linkIt = this->links.find(linkID);
+    if (linkIt == this->links.end())
+      continue;
+
+    if (_enabled)
+      MakeLinkColliderStatic(linkIt->second.get());
+    else
+      MakeLinkColliderDynamic(linkIt->second.get());
+  }
+
+  for (const auto nestedModelID : model->nestedModelEntityIds)
+  {
+    auto nestedModelIt = this->models.find(nestedModelID);
+    if (nestedModelIt == this->models.end())
+      continue;
+    auto nestedIdentity = this->GenerateIdentity(
+        nestedModelID, nestedModelIt->second);
+    this->SetFreeGroupStaticState(nestedIdentity, _enabled);
+  }
 }
 
 /////////////////////////////////////////////////
diff --git a/bullet/src/FreeGroupFeatures.cc b/bullet/src/FreeGroupFeatures.cc
index 930d97fb8..2e2dfdee8 100644
--- a/bullet/src/FreeGroupFeatures.cc
+++ b/bullet/src/FreeGroupFeatures.cc
@@ -17,10 +17,17 @@
 
 #include "FreeGroupFeatures.hh"
 
+#include <unordered_map>
+
 namespace gz {
 namespace physics {
 namespace bullet {
 
+namespace
+{
+std::unordered_map<std::size_t, bool> gravityStates;
+}  // namespace
+
 /////////////////////////////////////////////////
 Identity FreeGroupFeatures::FindFreeGroupForModel(
     const Identity &_modelID) const
@@ -54,9 +61,110 @@ Identity FreeGroupFeatures::FindFreeGroupForLink(
 Identity FreeGroupFeatures::GetFreeGroupRootLink(
     const Identity &_groupID) const
 {
-  (void) _groupID;
-  // Todo(Lobotuerk) implement canonical links
-  return this->GenerateInvalidId();
+  const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (model == nullptr || model->links.empty())
+    return this->GenerateInvalidId();
+
+  const auto rootLinkID = model->links.front();
+  auto linkIt = this->links.find(rootLinkID);
+  if (linkIt == this->links.end() || linkIt->second == nullptr)
+    return this->GenerateInvalidId();
+
+  return this->GenerateIdentity(rootLinkID, linkIt->second);
+}
+
+/////////////////////////////////////////////////
+void FreeGroupFeatures::SetFreeGroupGravityEnabled(
+    const Identity &_groupID, bool _enabled)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (model == nullptr)
+    return;
+
+  btVector3 gravity(0, 0, 0);
+  if (_enabled)
+  {
+    auto worldIt = this->worlds.find(static_cast<std::size_t>(model->world));
+    if (worldIt != this->worlds.end() && worldIt->second &&
+        worldIt->second->world)
+    {
+      gravity = worldIt->second->world->getGravity();
+    }
+  }
+
+  for (const auto linkID : model->links)
+  {
+    auto linkIt = this->links.find(linkID);
+    if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link)
+      continue;
+
+    auto body = linkIt->second->link.get();
+    body->setGravity(gravity);
+    if (!_enabled)
+      body->setActivationState(ACTIVE_TAG);
+  }
+
+  gravityStates[static_cast<std::size_t>(_groupID)] = _enabled;
+}
+
+/////////////////////////////////////////////////
+bool FreeGroupFeatures::GetFreeGroupStaticState(
+    const Identity &_groupID)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (model == nullptr)
+    return false;
+
+  return model->fixed;
+}
+
+/////////////////////////////////////////////////
+void FreeGroupFeatures::SetFreeGroupStaticState(
+    const Identity &_groupID,
+    bool _enabled)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (model == nullptr)
+    return;
+
+  model->fixed = _enabled;
+  for (const auto linkID : model->links)
+  {
+    auto linkIt = this->links.find(linkID);
+    if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link)
+      continue;
+
+    auto body = linkIt->second->link.get();
+    if (_enabled)
+    {
+      body->setMassProps(0, btVector3(0, 0, 0));
+      body->setCollisionFlags(
+          body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
+      body->setLinearVelocity(btVector3(0, 0, 0));
+      body->setAngularVelocity(btVector3(0, 0, 0));
+      body->setActivationState(DISABLE_SIMULATION);
+    }
+    else
+    {
+      body->setCollisionFlags(
+          body->getCollisionFlags() & ~btCollisionObject::CF_STATIC_OBJECT);
+      auto *linkInfo = this->ReferenceInterface<LinkInfo>(_groupID);
+      if (linkInfo != nullptr)
+        body->setMassProps(linkInfo->mass, linkInfo->inertia);
+      body->setActivationState(ACTIVE_TAG);
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
+    const Identity &_groupID)
+{
+  auto it = gravityStates.find(static_cast<std::size_t>(_groupID));
+  if (it != gravityStates.end())
+    return it->second;
+
+  return true;
 }
 
 /////////////////////////////////////////////////
@@ -79,6 +187,48 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   }
 }
 
+void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
+    const Identity &_groupID,
+    const LinearVelocity &_linearVelocity)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (model == nullptr)
+    return;
+
+  const btVector3 velocity = convertVec(_linearVelocity);
+  for (const auto linkID : model->links)
+  {
+    auto linkIt = this->links.find(linkID);
+    if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link)
+      continue;
+
+    auto body = linkIt->second->link.get();
+    body->setLinearVelocity(velocity);
+    body->setActivationState(ACTIVE_TAG);
+  }
+}
+
+void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
+    const Identity &_groupID,
+    const AngularVelocity &_angularVelocity)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (model == nullptr)
+    return;
+
+  const btVector3 angular = convertVec(_angularVelocity);
+  for (const auto linkID : model->links)
+  {
+    auto linkIt = this->links.find(linkID);
+    if (linkIt == this->links.end() || !linkIt->second || !linkIt->second->link)
+      continue;
+
+    auto body = linkIt->second->link.get();
+    body->setAngularVelocity(angular);
+    body->setActivationState(ACTIVE_TAG);
+  }
+}
+
 }  // namespace bullet
 }  // namespace physics
 }  // namespace gz
diff --git a/bullet/src/FreeGroupFeatures.hh b/bullet/src/FreeGroupFeatures.hh
index 278f066b5..10f9d5c51 100644
--- a/bullet/src/FreeGroupFeatures.hh
+++ b/bullet/src/FreeGroupFeatures.hh
@@ -27,8 +27,13 @@ namespace physics {
 namespace bullet {
 
 struct FreeGroupFeatureList : gz::physics::FeatureList<
+  GetFreeGroupStaticState,
+  GetFreeGroupGravityEnabled,
   FindFreeGroupFeature,
-  SetFreeGroupWorldPose
+  SetFreeGroupWorldPose,
+  SetFreeGroupWorldVelocity,
+  SetFreeGroupStaticState,
+  SetFreeGroupGravityEnabled
 > { };
 
 class FreeGroupFeatures
@@ -42,10 +47,33 @@ class FreeGroupFeatures
 
   Identity GetFreeGroupRootLink(const Identity &_groupID) const override;
 
+  void SetFreeGroupStaticState(
+      const Identity &_groupID,
+      bool _state) override;
+
+  bool GetFreeGroupStaticState(
+      const Identity &_groupID) override;
+
+  void SetFreeGroupGravityEnabled(
+      const Identity &_groupID,
+      bool _enabled) override;
+
+  bool GetFreeGroupGravityEnabled(
+      const Identity &_groupID) override;
+
   // ----- SetFreeGroupWorldPose -----
   void SetFreeGroupWorldPose(
       const Identity &_groupID,
       const PoseType &_pose) override;
+
+  // ----- SetFreeGroupWorldVelocity -----
+  void SetFreeGroupWorldLinearVelocity(
+      const Identity &_groupID,
+      const LinearVelocity &_linearVelocity) override;
+
+  void SetFreeGroupWorldAngularVelocity(
+      const Identity &_groupID,
+      const AngularVelocity &_angularVelocity) override;
 };
 
 }  // namespace bullet
diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc
index 4b1a9b9f3..d6e5bc27f 100644
--- a/test/common_test/simulation_features.cc
+++ b/test/common_test/simulation_features.cc
@@ -1285,7 +1285,6 @@ TYPED_TEST(SimulationFeaturesTestFreeGroupPhysics, FreeGroup)
   for (const std::string &name : this->pluginNames)
   {
     CHECK_UNSUPPORTED_ENGINE(name, "tpe")
-    CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone")
 
     auto world = LoadPluginAndWorld<FreeGroupPhysicsFeatures>(
       this->loader,
@@ -1396,7 +1395,6 @@ TYPED_TEST(SimulationFeaturesTestFreeGroupPhysics, FreeGroupNested)
   for (const std::string &name : this->pluginNames)
   {
     CHECK_UNSUPPORTED_ENGINE(name, "tpe")
-    CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone")
 
     auto world = LoadPluginAndWorld<FreeGroupPhysicsFeatures>(
       this->loader,

From 3e46813bdf8a2053732b55b33e258b2ebeee0abf Mon Sep 17 00:00:00 2001
From: Alejandro Hernandez Cordero <ahcorde@gmail.com>
Date: Thu, 5 Mar 2026 10:53:10 +0100
Subject: [PATCH 2/2] Support isStatic and GravityEnable in bullet and
 bullet-fetherstone

Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>
---
 bullet-featherstone/src/Base.hh              |  4 +
 bullet-featherstone/src/FreeGroupFeatures.cc | 35 ++++++--
 bullet-featherstone/src/FreeGroupFeatures.hh |  4 +-
 bullet-featherstone/src/SDFFeatures.cc       | 20 ++++-
 bullet/src/FreeGroupFeatures.cc              | 93 ++++++++++++++++----
 bullet/src/FreeGroupFeatures.hh              |  4 +-
 bullet/src/KinematicsFeatures.cc             | 16 +++-
 dartsim/src/FreeGroupFeatures.cc             |  7 +-
 dartsim/src/FreeGroupFeatures.hh             |  4 +-
 include/gz/physics/FreeGroup.hh              | 16 +++-
 include/gz/physics/detail/FreeGroup.hh       |  4 +-
 test/common_test/simulation_features.cc      |  2 +-
 12 files changed, 165 insertions(+), 44 deletions(-)

diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh
index aaaa159d5..5c037913d 100644
--- a/bullet-featherstone/src/Base.hh
+++ b/bullet-featherstone/src/Base.hh
@@ -144,6 +144,8 @@ struct ModelInfo
   Eigen::Isometry3d rootLinkToModelTf;
   Eigen::Isometry3d baseInertiaToLinkFrame;
   std::shared_ptr<GzMultiBody> body;
+  double baseMass = 0.0;
+  btVector3 baseInertia = btVector3(0, 0, 0);
 
   bool isNestedModel = false;
 
@@ -200,6 +202,8 @@ struct LinkInfo
   std::optional<int> indexInModel;
   Identity model;
   Eigen::Isometry3d inertiaToLinkFrame;
+  double mass = 0.0;
+  Eigen::Vector3d inertia = Eigen::Vector3d::Zero();
   std::unique_ptr<GzMultiBodyLinkCollider> collider = nullptr;
   std::unique_ptr<btCompoundShape> shape = nullptr;
   std::vector<std::size_t> collisionEntityIds = {};
diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
index 7382ae61f..57d19777f 100644
--- a/bullet-featherstone/src/FreeGroupFeatures.cc
+++ b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -32,7 +32,6 @@ void MakeLinkColliderStatic(LinkInfo *_linkInfo)
   if (!_linkInfo || !_linkInfo->collider)
     return;
 
-  std::cout << "MakeLinkColliderStatic: " << _linkInfo->name << " " << _linkInfo->isStaticOrFixed << std::endl;
   if (_linkInfo->isStaticOrFixed)
     return;
 
@@ -58,7 +57,6 @@ void MakeLinkColliderDynamic(LinkInfo *_linkInfo)
   if (!proxy)
     return;
 
-  std::cout << "MakeLinkColliderDynamic: " << _linkInfo->name << " " << _linkInfo->isStaticOrFixed << std::endl;
   if (_linkInfo->isStaticOrFixed)
     return;
 
@@ -190,8 +188,11 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
   // Free groups in bullet-featherstone are always represented by ModelInfo
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
 
-  if (model)
+  if (model && model->body)
   {
+    // Do not set velocity on a static (fixed-base) model
+    if (model->body->hasFixedBase())
+      return;
     model->body->setBaseOmega(convertVec(_angularVelocity));
     model->body->wakeUp();
   }
@@ -204,8 +205,11 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
   // Free groups in bullet-featherstone are always represented by ModelInfo
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   // Set Base Vel
-  if (model)
+  if (model && model->body)
   {
+    // Do not set velocity on a static (fixed-base) model
+    if (model->body->hasFixedBase())
+      return;
     model->body->setBaseVel(convertVec(_linearVelocity));
     model->body->wakeUp();
   }
@@ -254,7 +258,7 @@ void FreeGroupFeatures::SetFreeGroupGravityEnabled(
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
-    const Identity &_groupID)
+    const Identity &_groupID) const
 {
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   if (!model)
@@ -283,7 +287,7 @@ bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupStaticState(
-    const Identity &_groupID)
+    const Identity &_groupID) const
 {
   const auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   if (!model)
@@ -318,8 +322,25 @@ void FreeGroupFeatures::SetFreeGroupStaticState(
 
   if (model->body)
   {
+    // setFixedBase toggles the fixed-base flag and updates the base
+    // collider's dynamic type (CF_STATIC_OBJECT / CF_DYNAMIC_OBJECT).
+    // The Featherstone ABA checks this flag every step, so no need to
+    // remove/re-add the multibody from the world.
     model->body->setFixedBase(_enabled);
-    std::cout << "SetFreeGroupStaticState: " << _enabled << std::endl;
+
+    if (_enabled)
+    {
+      // btMultiBody::stepPositionsMultiDof still integrates existing
+      // base velocities even for a static base (it only guards on
+      // isBaseKinematic, not isBaseStaticOrKinematic).  Zero them
+      // explicitly so the body doesn't drift.
+      model->body->setBaseVel(btVector3(0, 0, 0));
+      model->body->setBaseOmega(btVector3(0, 0, 0));
+      model->body->clearForcesAndTorques();
+      model->body->clearConstraintForces();
+      model->body->clearVelocities();
+    }
+
     model->body->wakeUp();
   }
 
diff --git a/bullet-featherstone/src/FreeGroupFeatures.hh b/bullet-featherstone/src/FreeGroupFeatures.hh
index 5fd258367..858c82c1e 100644
--- a/bullet-featherstone/src/FreeGroupFeatures.hh
+++ b/bullet-featherstone/src/FreeGroupFeatures.hh
@@ -57,14 +57,14 @@ class FreeGroupFeatures
       bool _state) override;
 
   bool GetFreeGroupStaticState(
-      const Identity &_groupID) override;
+      const Identity &_groupID) const override;
 
   void SetFreeGroupGravityEnabled(
       const Identity &_groupID,
       bool _enabled) override;
 
   bool GetFreeGroupGravityEnabled(
-      const Identity &_groupID) override;
+      const Identity &_groupID) const override;
 
   // ----- SetFreeGroupWorldVelocity -----
   void SetFreeGroupWorldLinearVelocity(
diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
index 7db2ceb77..8993d1353 100644
--- a/bullet-featherstone/src/SDFFeatures.cc
+++ b/bullet-featherstone/src/SDFFeatures.cc
@@ -617,9 +617,9 @@ Identity SDFFeatures::ConstructSdfModelImpl(
     });
   rootMultiBody->setUserIndex(std::size_t(rootID));
 
-  auto modelID =
+    auto modelID =
       this->GenerateIdentity(rootModelID, this->models[rootModelID]);
-  const auto *model = this->ReferenceInterface<ModelInfo>(modelID);
+    auto *model = this->ReferenceInterface<ModelInfo>(modelID);
 
   // Add world joint
   if (structure.rootJoint)
@@ -664,6 +664,15 @@ Identity SDFFeatures::ConstructSdfModelImpl(
       linkIDs.insert(std::make_pair(link, linkID));
     }
 
+    if (auto *linkInfo = this->ReferenceInterface<LinkInfo>(linkIDs.at(link)))
+    {
+      linkInfo->mass = static_cast<double>(mass);
+      linkInfo->inertia = Eigen::Vector3d(
+        static_cast<double>(inertia[0]),
+        static_cast<double>(inertia[1]),
+        static_cast<double>(inertia[2]));
+    }
+
     if (!structure.parentOf.empty())
     {
       const auto &parentInfo = structure.parentOf.at(link);
@@ -912,6 +921,13 @@ Identity SDFFeatures::ConstructSdfModelImpl(
     extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose);
     model->body->setBaseMass(mass);
     model->body->setBaseInertia(inertia);
+    model->baseMass = mass;
+    model->baseInertia = inertia;
+    if (auto *linkInfo = this->ReferenceInterface<LinkInfo>(rootID))
+    {
+      linkInfo->mass = static_cast<double>(mass);
+      linkInfo->inertia = Eigen::Vector3d(inertia[0], inertia[1], inertia[2]);
+    }
   }
 
   world->world->addMultiBody(model->body.get());
diff --git a/bullet/src/FreeGroupFeatures.cc b/bullet/src/FreeGroupFeatures.cc
index 2e2dfdee8..fbf79c24c 100644
--- a/bullet/src/FreeGroupFeatures.cc
+++ b/bullet/src/FreeGroupFeatures.cc
@@ -101,7 +101,14 @@ void FreeGroupFeatures::SetFreeGroupGravityEnabled(
     auto body = linkIt->second->link.get();
     body->setGravity(gravity);
     if (!_enabled)
-      body->setActivationState(ACTIVE_TAG);
+    {
+      // Zero out velocities and forces so the body doesn't drift
+      // from residual momentum when gravity is disabled
+      body->setLinearVelocity(btVector3(0, 0, 0));
+      body->setAngularVelocity(btVector3(0, 0, 0));
+      body->clearForces();
+    }
+    body->setActivationState(ACTIVE_TAG);
   }
 
   gravityStates[static_cast<std::size_t>(_groupID)] = _enabled;
@@ -109,7 +116,7 @@ void FreeGroupFeatures::SetFreeGroupGravityEnabled(
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupStaticState(
-    const Identity &_groupID)
+    const Identity &_groupID) const
 {
   auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
   if (model == nullptr)
@@ -142,15 +149,16 @@ void FreeGroupFeatures::SetFreeGroupStaticState(
           body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
       body->setLinearVelocity(btVector3(0, 0, 0));
       body->setAngularVelocity(btVector3(0, 0, 0));
+      body->clearForces();
       body->setActivationState(DISABLE_SIMULATION);
     }
     else
     {
       body->setCollisionFlags(
           body->getCollisionFlags() & ~btCollisionObject::CF_STATIC_OBJECT);
-      auto *linkInfo = this->ReferenceInterface<LinkInfo>(_groupID);
-      if (linkInfo != nullptr)
-        body->setMassProps(linkInfo->mass, linkInfo->inertia);
+      // Restore mass/inertia from this link's own LinkInfo
+      body->setMassProps(linkIt->second->mass, linkIt->second->inertia);
+      body->updateInertiaTensor();
       body->setActivationState(ACTIVE_TAG);
     }
   }
@@ -158,7 +166,7 @@ void FreeGroupFeatures::SetFreeGroupStaticState(
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
-    const Identity &_groupID)
+    const Identity &_groupID) const
 {
   auto it = gravityStates.find(static_cast<std::size_t>(_groupID));
   if (it != gravityStates.end())
@@ -172,18 +180,63 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
     const Identity &_groupID,
     const PoseType &_pose)
 {
-  // Convert pose
-  const auto poseTranslation = _pose.translation();
-  const auto poseLinear = _pose.linear();
-  btTransform baseTransform;
-  baseTransform.setOrigin(convertVec(poseTranslation));
-  baseTransform.setBasis(convertMat(poseLinear));
-
-  // Set base transform
   const auto &model = this->models.at(_groupID);
-  for (auto link : model->links)
+  if (model->links.empty())
+    return;
+
+  // Get the current world pose of the root link (link frame, not COM frame)
+  const auto rootLinkID = model->links.front();
+  const auto &rootLinkInfo = this->links.at(rootLinkID);
+  btTransform currentRootCOM =
+    rootLinkInfo->link->getCenterOfMassTransform();
+
+  // Root link world pose = currentRootCOM * inertialPose.Inverse()
+  const auto rootInertialPose =
+    gz::math::eigen3::convert(rootLinkInfo->inertialPose);
+  btTransform rootInertialBt;
+  rootInertialBt.setOrigin(convertVec(rootInertialPose.translation()));
+  rootInertialBt.setBasis(convertMat(rootInertialPose.linear()));
+
+  btTransform currentRootWorldPose = currentRootCOM * rootInertialBt.inverse();
+
+  // Desired new world pose for the root link
+  btTransform newRootWorldPose;
+  newRootWorldPose.setOrigin(convertVec(_pose.translation()));
+  newRootWorldPose.setBasis(convertMat(_pose.linear()));
+
+  // Compute the relative transform change
+  btTransform tfChange = newRootWorldPose * currentRootWorldPose.inverse();
+
+  // Apply the transform change to all links
+  for (const auto linkID : model->links)
   {
-    this->links.at(link)->link->setCenterOfMassTransform(baseTransform);
+    const auto &linkInfo = this->links.at(linkID);
+    auto body = linkInfo->link.get();
+
+    // Current link world pose (link frame)
+    btTransform linkInertialBt;
+    const auto linkInertialPose =
+      gz::math::eigen3::convert(linkInfo->inertialPose);
+    linkInertialBt.setOrigin(convertVec(linkInertialPose.translation()));
+    linkInertialBt.setBasis(convertMat(linkInertialPose.linear()));
+
+    btTransform currentLinkCOM = body->getCenterOfMassTransform();
+    btTransform currentLinkWorld = currentLinkCOM * linkInertialBt.inverse();
+
+    // Apply the change to get new link world pose, then convert back to COM
+    btTransform newLinkWorld = tfChange * currentLinkWorld;
+    btTransform newLinkCOM = newLinkWorld * linkInertialBt;
+
+    body->setCenterOfMassTransform(newLinkCOM);
+    // Also update the motion state so Bullet doesn't reset the pose
+    if (body->getMotionState())
+    {
+      body->getMotionState()->setWorldTransform(newLinkCOM);
+    }
+    body->setLinearVelocity(btVector3(0, 0, 0));
+    body->setAngularVelocity(btVector3(0, 0, 0));
+    body->clearForces();
+    body->setActivationState(ACTIVE_TAG);
   }
 }
 
@@ -195,6 +248,10 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
   if (model == nullptr)
     return;
 
+  // Do not set velocity on a static model
+  if (model->fixed)
+    return;
+
   const btVector3 velocity = convertVec(_linearVelocity);
   for (const auto linkID : model->links)
   {
@@ -216,6 +273,10 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
   if (model == nullptr)
     return;
 
+  // Do not set velocity on a static model
+  if (model->fixed)
+    return;
+
   const btVector3 angular = convertVec(_angularVelocity);
   for (const auto linkID : model->links)
   {
diff --git a/bullet/src/FreeGroupFeatures.hh b/bullet/src/FreeGroupFeatures.hh
index 10f9d5c51..5b26c7f13 100644
--- a/bullet/src/FreeGroupFeatures.hh
+++ b/bullet/src/FreeGroupFeatures.hh
@@ -52,14 +52,14 @@ class FreeGroupFeatures
       bool _state) override;
 
   bool GetFreeGroupStaticState(
-      const Identity &_groupID) override;
+      const Identity &_groupID) const  override;
 
   void SetFreeGroupGravityEnabled(
       const Identity &_groupID,
       bool _enabled) override;
 
   bool GetFreeGroupGravityEnabled(
-      const Identity &_groupID) override;
+      const Identity &_groupID) const override;
 
   // ----- SetFreeGroupWorldPose -----
   void SetFreeGroupWorldPose(
diff --git a/bullet/src/KinematicsFeatures.cc b/bullet/src/KinematicsFeatures.cc
index 38057c3d3..4d3243fa7 100644
--- a/bullet/src/KinematicsFeatures.cc
+++ b/bullet/src/KinematicsFeatures.cc
@@ -37,12 +37,22 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
     return data;
   }
 
-  const auto linkID = _id.ID();
+  auto linkID = _id.ID();
 
+  // If the ID doesn't correspond to a link, it may be a model (e.g. from a
+  // FreeGroup). Resolve to the model's root link.
   if (this->links.find(linkID) == this->links.end())
   {
-    gzerr << "Given a FrameID not belonging to a link.\n";
-    return data;
+    const auto modelIt = this->models.find(linkID);
+    if (modelIt != this->models.end() && !modelIt->second->links.empty())
+    {
+      linkID = modelIt->second->links.front();
+    }
+    else
+    {
+      gzerr << "Given a FrameID not belonging to a link or model.\n";
+      return data;
+    }
   }
   const auto &linkInfo = this->links.at(linkID);
   const auto &rigidBody = linkInfo->link;
diff --git a/dartsim/src/FreeGroupFeatures.cc b/dartsim/src/FreeGroupFeatures.cc
index 5276da3a4..894e56536 100644
--- a/dartsim/src/FreeGroupFeatures.cc
+++ b/dartsim/src/FreeGroupFeatures.cc
@@ -165,7 +165,8 @@ void FreeGroupFeatures::SetFreeGroupStaticState(
   {
     auto nestedModelIdentity = this->GenerateIdentity(nestedModel);
     const auto &nestedModelInfo = this->models.at(nestedModel);
-    // Only set static state if the nested model has BodyNodes or further nested models
+    // Only set static state if the nested model has BodyNodes or
+    // further nested models
     if (nestedModelInfo->model->getNumBodyNodes() > 0 ||
         !nestedModelInfo->nestedModels.empty())
     {
@@ -176,7 +177,7 @@ void FreeGroupFeatures::SetFreeGroupStaticState(
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupStaticState(
-    const Identity &_groupID)
+    const Identity &_groupID) const
 {
   const auto modelInfo = this->models.at(_groupID);
   // Verify that the model qualifies as a FreeGroup
@@ -239,7 +240,7 @@ void FreeGroupFeatures::SetFreeGroupGravityEnabled(
 
 /////////////////////////////////////////////////
 bool FreeGroupFeatures::GetFreeGroupGravityEnabled(
-    const Identity &_groupID)
+    const Identity &_groupID) const
 {
   const FreeGroupInfo &info = GetCanonicalInfo(_groupID);
   if (!info.model)
diff --git a/dartsim/src/FreeGroupFeatures.hh b/dartsim/src/FreeGroupFeatures.hh
index 1925856a5..86cae455c 100644
--- a/dartsim/src/FreeGroupFeatures.hh
+++ b/dartsim/src/FreeGroupFeatures.hh
@@ -69,14 +69,14 @@ class FreeGroupFeatures
       bool _state) override;
 
   bool GetFreeGroupStaticState(
-      const Identity &_groupID) override;
+      const Identity &_groupID) const override;
 
   void SetFreeGroupGravityEnabled(
       const Identity &_groupID,
       bool _enabled) override;
 
   bool GetFreeGroupGravityEnabled(
-      const Identity &_groupID) override;
+      const Identity &_groupID) const override;
 
   void SetFreeGroupWorldLinearVelocity(
       const Identity &_groupID,
diff --git a/include/gz/physics/FreeGroup.hh b/include/gz/physics/FreeGroup.hh
index 195bb213c..dcfdc4e1d 100644
--- a/include/gz/physics/FreeGroup.hh
+++ b/include/gz/physics/FreeGroup.hh
@@ -205,14 +205,18 @@ namespace gz
       class FreeGroup : public virtual Entity<PolicyT, FeaturesT>
       {
         /// \brief Get this FreeGroup static state.
-        public: bool GetStaticState();
+        public: bool GetStaticState() const;
       };
 
+      /// \brief Implementation interface for the FreeGroup feature.
+      ///
+      /// This class defines the interface that physics engine plugins must
+      /// implement to support the FreeGroup feature functionality.
       public: template <typename PolicyT>
       class Implementation : public virtual Feature::Implementation<PolicyT>
       {
         public: virtual bool GetFreeGroupStaticState(
-            const Identity &_groupID) = 0;
+            const Identity &_groupID) const = 0;
       };
     };
 
@@ -235,6 +239,10 @@ namespace gz
         public: void SetGravityEnabled(bool _enabled);
       };
 
+      /// \brief Implementation interface for the FreeGroup feature.
+      ///
+      /// This class defines the interface that physics engine plugins must
+      /// implement to support the FreeGroup feature functionality.
       public: template <typename PolicyT>
       class Implementation : public virtual Feature::Implementation<PolicyT>
       {
@@ -260,14 +268,14 @@ namespace gz
       class FreeGroup : public virtual Entity<PolicyT, FeaturesT>
       {
         /// \brief Get this FreeGroup Gravity enabled.
-        public: bool GetGravityEnabled();
+        public: bool GetGravityEnabled() const;
       };
 
       public: template <typename PolicyT>
       class Implementation : public virtual Feature::Implementation<PolicyT>
       {
         public: virtual bool GetFreeGroupGravityEnabled(
-            const Identity &_groupID) = 0;
+            const Identity &_groupID) const = 0;
       };
     };
 
diff --git a/include/gz/physics/detail/FreeGroup.hh b/include/gz/physics/detail/FreeGroup.hh
index 8496650a3..6c879a74c 100644
--- a/include/gz/physics/detail/FreeGroup.hh
+++ b/include/gz/physics/detail/FreeGroup.hh
@@ -121,7 +121,7 @@ namespace gz
     /////////////////////////////////////////////////
     template <typename PolicyT, typename FeaturesT>
     bool GetFreeGroupStaticState::FreeGroup<PolicyT, FeaturesT>::
-    GetStaticState()
+    GetStaticState() const
     {
       return this->template Interface<GetFreeGroupStaticState>()
         ->GetFreeGroupStaticState(this->identity);
@@ -139,7 +139,7 @@ namespace gz
     /////////////////////////////////////////////////
     template <typename PolicyT, typename FeaturesT>
     bool GetFreeGroupGravityEnabled::FreeGroup<PolicyT, FeaturesT>::
-    GetGravityEnabled()
+    GetGravityEnabled() const
     {
       return this->template Interface<GetFreeGroupGravityEnabled>()
         ->GetFreeGroupGravityEnabled(this->identity);
diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc
index d6e5bc27f..8e6dcdc68 100644
--- a/test/common_test/simulation_features.cc
+++ b/test/common_test/simulation_features.cc
@@ -1276,7 +1276,7 @@ template <class T>
 class SimulationFeaturesTestFreeGroupPhysics :
   public SimulationFeaturesTest<T>{};
 using SimulationFeaturesTestFreeGroupPhysicsTypes =
-  ::testing::Types<FreeGroupFeatures>;
+  ::testing::Types<FreeGroupPhysicsFeatures>;
 TYPED_TEST_SUITE(SimulationFeaturesTestFreeGroupPhysics,
                  SimulationFeaturesTestFreeGroupPhysicsTypes);