Enable/disable: gravity and static (Bullet and Bullet-featherstone)

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);
 };
 
@@ -141,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;
 
@@ -197,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 = {};

bullet-featherstone/src/FreeGroupFeatures.cc

@@ -24,6 +24,56 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+namespace
+{
+/////////////////////////////////////////////////
+void MakeLinkColliderStatic(LinkInfo *_linkInfo)
+{
+  if (!_linkInfo || !_linkInfo->collider)
+    return;
+
+  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;
+
+  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)
 {
@@ -138,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();
   }
@@ -152,13 +205,168 @@ 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();
   }
 }
 
+/////////////////////////////////////////////////
+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
+{
+  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
+{
+  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;
+}
+
+/////////////////////////////////////////////////
+void FreeGroupFeatures::SetFreeGroupStaticState(
+    const Identity &_groupID,
+    bool _enabled)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_groupID);
+  if (!model)
+    return;
+
+  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);
+
+    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();
+  }
+
+  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);
+  }
+}
+
 /////////////////////////////////////////////////
 void FreeGroupFeatures::SetFreeGroupWorldPose(
     const Identity &_groupID,

bullet-featherstone/src/FreeGroupFeatures.hh

@@ -48,6 +48,20 @@ class FreeGroupFeatures
       const Identity &_groupID,
       const PoseType &_pose) override;
 
+  void SetFreeGroupStaticState(
+      const Identity &_groupID,
+      bool _state) override;
+
+  bool GetFreeGroupStaticState(
+      const Identity &_groupID) const override;
+
+  void SetFreeGroupGravityEnabled(
+      const Identity &_groupID,
+      bool _enabled) override;
+
+  bool GetFreeGroupGravityEnabled(
+      const Identity &_groupID) const override;
+
   // ----- SetFreeGroupWorldVelocity -----
   void SetFreeGroupWorldLinearVelocity(
       const Identity &_groupID,

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());