bullet-featherstone: Update fixed constraint behavior (#632)

* bullet-featherstone fixed joint implementation now calls setMaxAppliedImpulse to enforce fixed constraint.
*When the pose of the parent body of a fixed constraint is changed by FreeGroupSetWorldPose, the child pose is updated to enforce pose transform relative to the parent
* prevent collision between parent and child link if parent is static

Signed-off-by: Ian Chen <ichen@openrobotics.org>
Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>

bullet-featherstone/src/Base.hh

@@ -112,6 +112,24 @@ struct ModelInfo
   }
 };
 
+/// \brief Custom GzMultiBodyLinkCollider class
+class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider {
+  using btMultiBodyLinkCollider::btMultiBodyLinkCollider;
+
+  /// \brief Overrides base function to enable support for ignoring
+  /// collision with objects from other bodies if
+  /// btCollisionObject::setIgnoreCollisionCheck is called.
+  /// Note: originally btMultiBodyLinkCollider::checkCollideWithOverride
+  /// just returns true if the input collision object is from a
+  /// different body and disregards any setIgnoreCollisionCheck calls.
+  public: bool checkCollideWithOverride(const btCollisionObject *_co) const
+          override
+  {
+    return btMultiBodyLinkCollider::checkCollideWithOverride(_co) &&
+           btCollisionObject::checkCollideWithOverride(_co);
+  }
+};
+
 /// Link information is embedded inside the model, so all we need to store here
 /// is a reference to the model and the index of this link inside of it.
 struct LinkInfo
@@ -120,10 +138,12 @@ struct LinkInfo
   std::optional<int> indexInModel;
   Identity model;
   Eigen::Isometry3d inertiaToLinkFrame;
-  std::unique_ptr<btMultiBodyLinkCollider> collider = nullptr;
+  std::unique_ptr<GzMultiBodyLinkCollider> collider = nullptr;
   std::unique_ptr<btCompoundShape> shape = nullptr;
   std::vector<std::size_t> collisionEntityIds = {};
   std::unordered_map<std::string, std::size_t> collisionNameToEntityId = {};
+  // Link is either static, fixed to world, or has zero dofs
+  bool isStaticOrFixed = false;
 };
 
 struct CollisionInfo

bullet-featherstone/src/FreeGroupFeatures.cc

@@ -18,11 +18,67 @@
 #include "FreeGroupFeatures.hh"
 
 #include <memory>
+#include <unordered_map>
 
 namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/////////////////////////////////////////////////
+btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex)
+{
+  if (_linkIndex == -1)
+  {
+    return _body->getBaseWorldTransform();
+  }
+  else
+  {
+    btMultiBodyLinkCollider *collider = _body->getLinkCollider(_linkIndex);
+    return collider->getWorldTransform();
+  }
+}
+
+/////////////////////////////////////////////////
+void enforceFixedConstraint(
+    btMultiBodyFixedConstraint *_fixedConstraint,
+    const std::unordered_map<std::size_t, Base::JointInfoPtr> &_allJoints)
+{
+  // Update fixed constraint's child link pose to maintain a fixed transform
+  // from the parent link.
+  btMultiBody *parent = _fixedConstraint->getMultiBodyA();
+  btMultiBody *child = _fixedConstraint->getMultiBodyB();
+
+  btTransform parentToChildTf;
+  parentToChildTf.setOrigin(_fixedConstraint->getPivotInA());
+  parentToChildTf.setBasis(_fixedConstraint->getFrameInA());
+
+  int parentLinkIndex = _fixedConstraint->getLinkA();
+  int childLinkIndex = _fixedConstraint->getLinkB();
+
+  btTransform parentLinkTf = getWorldTransformForLink(parent, parentLinkIndex);
+  btTransform childLinkTf = getWorldTransformForLink(child, childLinkIndex);
+
+  btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf;
+  btTransform childBaseTf =  child->getBaseWorldTransform();
+  btTransform childBaseToLink =
+      childBaseTf.inverse() * childLinkTf;
+  btTransform newChildBaseTf =
+      expectedChildLinkTf * childBaseToLink.inverse();
+  child->setBaseWorldTransform(newChildBaseTf);
+  for (const auto &joint : _allJoints)
+  {
+    if (joint.second->fixedConstraint)
+    {
+      // Recursively enforce constraints where the child here is a parent to
+      // other constraints.
+      if (joint.second->fixedConstraint->getMultiBodyA() == child)
+      {
+        enforceFixedConstraint(joint.second->fixedConstraint.get(), _allJoints);
+      }
+    }
+  }
+}
+
 /////////////////////////////////////////////////
 Identity FreeGroupFeatures::FindFreeGroupForModel(
     const Identity &_modelID) const
@@ -33,6 +89,20 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
   if (model->body->hasFixedBase())
     return this->GenerateInvalidId();
 
+  // Also reject if the model is a child of a fixed constraint
+  // (detachable joint)
+  for (const auto & joint : this->joints)
+  {
+    if (joint.second->fixedConstraint)
+    {
+      if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get())
+      {
+        return this->GenerateInvalidId();
+      }
+    }
+  }
+
+
   return _modelID;
 }
 
@@ -97,7 +167,25 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
   {
     model->body->setBaseWorldTransform(
         convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));
+
     model->body->wakeUp();
+
+    for (auto & joint : this->joints)
+    {
+      if (joint.second->fixedConstraint)
+      {
+        // Only the parent body of a fixed joint can be moved but we need to
+        // enforce the fixed constraint by updating the child pose so it
+        // remains at a fixed transform from parent. We do this recursively to
+        // account for other constraints attached to the child.
+        btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA();
+        if (parent == model->body.get())
+        {
+          enforceFixedConstraint(joint.second->fixedConstraint.get(),
+              this->joints);
+        }
+      }
+    }
   }
 }
 

bullet-featherstone/src/JointFeatures.cc

@@ -19,6 +19,7 @@
 
 #include <algorithm>
 #include <memory>
+#include <unordered_map>
 
 #include <gz/common/Console.hh>
 #include <sdf/Joint.hh>
@@ -27,6 +28,85 @@ namespace gz {
 namespace physics {
 namespace bullet_featherstone {
 
+/////////////////////////////////////////////////
+void makeColliderStatic(LinkInfo *_linkInfo)
+{
+  btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
+  if (!childCollider)
+    return;
+
+  // if link is already static or fixed, we do not need to change its
+  // collision flags
+  if (_linkInfo->isStaticOrFixed)
+    return;
+
+  btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
+  if (!childProxy)
+    return;
+
+  childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
+  childProxy->m_collisionFilterMask =
+      btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
+#if BT_BULLET_VERSION >= 307
+  childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+#endif
+}
+
+/////////////////////////////////////////////////
+void makeColliderDynamic(LinkInfo *_linkInfo)
+{
+  btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
+  if (!childCollider)
+    return;
+
+  btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
+  if (!childProxy)
+    return;
+
+  // If broadphase and collision object flags do not agree, the
+  // link was originally non-static but made static by AttachJoint
+  if (!_linkInfo->isStaticOrFixed &&
+      ((childProxy->m_collisionFilterGroup &
+      btBroadphaseProxy::StaticFilter) > 0))
+  {
+    childProxy->m_collisionFilterGroup =
+        btBroadphaseProxy::DefaultFilter;
+    childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
+#if BT_BULLET_VERSION >= 307
+    childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
+#endif
+  }
+}
+
+/////////////////////////////////////////////////
+void updateColliderFlagsRecursive(std::size_t _linkID,
+  const std::unordered_map<std::size_t, std::shared_ptr<JointInfo>> &_joints,
+  const std::unordered_map<std::size_t, std::shared_ptr<LinkInfo>> &_links,
+  std::function<void(LinkInfo *)> _updateColliderCb)
+{
+  btMultiBodyFixedConstraint *fixedConstraint = nullptr;
+  std::size_t childLinkID = 0u;
+  for (const auto &joint : _joints)
+  {
+    if (!joint.second->fixedConstraint)
+      continue;
+    if (!joint.second->parentLinkID.has_value() ||
+        joint.second->parentLinkID.value() != _linkID)
+      continue;
+
+    fixedConstraint =  joint.second->fixedConstraint.get();
+    childLinkID = std::size_t(joint.second->childLinkID);
+  }
+
+  if (!fixedConstraint)
+    return;
+
+  auto childInfo = _links.at(childLinkID);
+  _updateColliderCb(childInfo.get());
+
+  updateColliderFlagsRecursive(childLinkID, _joints, _links, _updateColliderCb);
+}
+
 /////////////////////////////////////////////////
 double JointFeatures::GetJointPosition(
     const Identity &_id, const std::size_t _dof) const
@@ -341,6 +421,31 @@ Identity JointFeatures::AttachFixedJoint(
   if (world != nullptr && world->world)
   {
     world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get());
+    jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9));
+
+    // Make child link static if parent is static
+    // This is done by updating collision flags
+    btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get();
+    btMultiBodyLinkCollider *childCollider = linkInfo->collider.get();
+    if (parentCollider && childCollider)
+    {
+      // disable collision between parent and child collider
+      // \todo(iche033) if self collide is true, extend this to
+      // disable collisions between all the links in the parent's model with
+      // all the links in the child's model.
+      parentCollider->setIgnoreCollisionCheck(childCollider, true);
+      childCollider->setIgnoreCollisionCheck(parentCollider, true);
+
+      // If parent link is static or fixed, recusively update child colliders
+      // collision flags to be static.
+      if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed)
+      {
+        makeColliderStatic(linkInfo);
+        updateColliderFlagsRecursive(std::size_t(_childID),
+            this->joints, this->links, makeColliderStatic);
+      }
+    }
+
     return this->GenerateIdentity(jointID, this->joints.at(jointID));
   }
 
@@ -353,13 +458,45 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
   auto jointInfo = this->ReferenceInterface<JointInfo>(_jointId);
   if (jointInfo->fixedConstraint)
   {
+    // Make links dynamic again as they were originally not static
+    // This is done by reverting any collision flag changes
+    // made in AttachJoint
+    auto *linkInfo = this->ReferenceInterface<LinkInfo>(jointInfo->childLinkID);
+    if (jointInfo->parentLinkID.has_value())
+    {
+      auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value());
+
+      btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get();
+      btMultiBodyLinkCollider *childCollider = linkInfo->collider.get();
+      if (parentCollider && childCollider)
+      {
+        parentCollider->setIgnoreCollisionCheck(childCollider, false);
+        childCollider->setIgnoreCollisionCheck(parentCollider, false);
+        btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
+        if (childProxy)
+        {
+          // Recursively make child colliders dynamic if they were originally
+          // not static
+          if (!linkInfo->isStaticOrFixed &&
+              ((childProxy->m_collisionFilterGroup &
+              btBroadphaseProxy::StaticFilter) > 0))
+          {
+            makeColliderDynamic(linkInfo);
+            updateColliderFlagsRecursive(std::size_t(jointInfo->childLinkID),
+                this->joints, this->links, makeColliderDynamic);
+          }
+        }
+      }
+    }
+
     auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
     if (modelInfo)
     {
       auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
       world->world->removeMultiBodyConstraint(jointInfo->fixedConstraint.get());
       jointInfo->fixedConstraint.reset();
       jointInfo->fixedConstraint = nullptr;
+      modelInfo->body->wakeUp();
     }
   }
 }

bullet-featherstone/src/SDFFeatures.cc

@@ -1263,7 +1263,7 @@ bool SDFFeatures::AddSdfCollision(
 
       // NOTE: Bullet does not appear to support different surface properties
       // for different shapes attached to the same link.
-      linkInfo->collider = std::make_unique<btMultiBodyLinkCollider>(
+      linkInfo->collider = std::make_unique<GzMultiBodyLinkCollider>(
         model->body.get(), linkIndexInModel);
 
       linkInfo->shape->addChildShape(btInertialToCollision, shape.get());
@@ -1330,6 +1330,12 @@ bool SDFFeatures::AddSdfCollision(
           linkInfo->collider.get(),
           btBroadphaseProxy::StaticFilter,
           btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+          linkInfo->isStaticOrFixed = true;
+
+        // Set collider collision flags
+#if BT_BULLET_VERSION >= 307
+        linkInfo->collider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+#endif
       }
       else
       {

test/common_test/CMakeLists.txt

@@ -76,6 +76,11 @@ foreach(test ${tests})
       "BT_BULLET_VERSION_LE_325"
     )
   endif()
+  if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.06)
+    target_compile_definitions(${test_executable} PRIVATE
+      "BT_BULLET_VERSION_LE_306"
+    )
+  endif()
 
   if (DART_HAS_CONTACT_SURFACE_HEADER)
     target_compile_definitions(${test_executable} PRIVATE DART_HAS_CONTACT_SURFACE)

test/common_test/Worlds.hh

@@ -36,6 +36,8 @@ const auto kFallingWorld = CommonTestWorld("falling.world");
 const auto kFallingAddedMassWorld = CommonTestWorld("falling_added_mass.world");
 const auto kGroundSdf = CommonTestWorld("ground.sdf");
 const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf");
+const auto kJointAcrossModelsFixedSdf =
+    CommonTestWorld("joint_across_models_fixed.sdf");
 const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf");
 const auto kMimicFastSlowPendulumsWorld =
   CommonTestWorld("mimic_fast_slow_pendulums_world.sdf");