bullet-featherstone: Update fixed constraint behavior

bullet-featherstone/src/Base.hh

@@ -124,6 +124,8 @@ struct LinkInfo
   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
@@ -171,6 +173,10 @@ struct JointInfo
   std::size_t indexInGzModel = 0;
   btScalar effort = 0;
 
+  // True if the fixed constraint's child link is welded to parent link as if
+  // they are part of the same body.
+  bool fixedConstraintWeldChildToParent = true;
+
   std::shared_ptr<btMultiBodyJointMotor> motor = nullptr;
   std::shared_ptr<btMultiBodyJointLimitConstraint> jointLimits = nullptr;
   std::shared_ptr<btMultiBodyFixedConstraint> fixedConstraint = nullptr;

bullet-featherstone/src/JointFeatures.cc

@@ -341,6 +341,26 @@ Identity JointFeatures::AttachFixedJoint(
   if (world != nullptr && world->world)
   {
     world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get());
+
+    // Make links static if parent or child link is static
+    // This is done by changing collision filter groups / masks
+    // Otherwise bullet will push bodies apart
+    btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get();
+    btMultiBodyLinkCollider *childCollider = linkInfo->collider.get();
+    if (parentCollider && childCollider)
+    {
+      if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed)
+      {
+        btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
+        if (childProxy)
+        {
+          childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
+          childProxy->m_collisionFilterMask =
+              btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
+        }
+      }
+    }
+
     return this->GenerateIdentity(jointID, this->joints.at(jointID));
   }
 
@@ -353,13 +373,43 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
   auto jointInfo = this->ReferenceInterface<JointInfo>(_jointId);
   if (jointInfo->fixedConstraint)
   {
+    // Make links dynamic again they were originally not static
+    // This is done by revert any collision flags / masks 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)
+      {
+        btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
+        if (childProxy)
+        {
+          // 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;
+          }
+        }
+      }
+    }
+
     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();
     }
   }
 }
@@ -380,6 +430,18 @@ void JointFeatures::SetJointTransformFromParent(
   }
 }
 
+/////////////////////////////////////////////////
+void JointFeatures::SetFixedJointWeldChildToParent(
+    const Identity &_id, bool _weldChildToParent)
+{
+  auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
+
+  if (jointInfo->fixedConstraint)
+  {
+    jointInfo->fixedConstraintWeldChildToParent = _weldChildToParent;
+  }
+}
+
 /////////////////////////////////////////////////
 Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame(
     const Identity &_id) const

bullet-featherstone/src/JointFeatures.hh

@@ -51,7 +51,7 @@ struct JointFeatureList : FeatureList<
 
   SetMimicConstraintFeature,
 
-  FixedJointCast
+  SetFixedJointWeldChildToParentFeature
 > { };
 
 class JointFeatures :
@@ -140,6 +140,10 @@ class JointFeatures :
   public: void SetJointTransformFromParent(
       const Identity &_id, const Pose3d &_pose) override;
 
+  // ----- SetFixedJointWeldChildToParentFeature -----
+  public: void SetFixedJointWeldChildToParent(
+    const Identity &_id, bool _weldChildToParent) override;
+
   // ----- Transmitted wrench -----
   public: Wrench3d GetJointTransmittedWrenchInJointFrame(
       const Identity &_id) const override;

bullet-featherstone/src/SDFFeatures.cc

@@ -1328,6 +1328,7 @@ bool SDFFeatures::AddSdfCollision(
           linkInfo->collider.get(),
           btBroadphaseProxy::StaticFilter,
           btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+          linkInfo->isStaticOrFixed = true;
       }
       else
       {

bullet-featherstone/src/SimulationFeatures.cc

@@ -43,6 +43,57 @@ void SimulationFeatures::WorldForwardStep(
     stepSize = dt.count();
   }
 
+  // Update fixed constraint behavior if fixedJointWeldChildToParent is true.
+  // Do this before stepping, i.e. before physics engine tries to solve and
+  // enforce the constraint
+  for (auto & joint : this->joints)
+  {
+    if (joint.second->fixedConstraint &&
+        joint.second->fixedConstraintWeldChildToParent)
+    {
+      // Update fxied constraint's child link pose to maintain a fixed transform
+      // from the parent link.
+      btTransform parentToChildTf;
+      parentToChildTf.setOrigin(joint.second->fixedConstraint->getPivotInA());
+      parentToChildTf.setBasis(joint.second->fixedConstraint->getFrameInA());
+      btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA();
+      btMultiBody *child = joint.second->fixedConstraint->getMultiBodyB();
+
+      int parentLinkIndex = joint.second->fixedConstraint->getLinkA();
+      int childLinkIndex = joint.second->fixedConstraint->getLinkB();
+
+      btTransform parentLinkTf;
+      btTransform childLinkTf;
+      if (parentLinkIndex == -1)
+      {
+        parentLinkTf = parent->getBaseWorldTransform();
+      }
+      else
+      {
+        btMultiBodyLinkCollider *parentCollider =
+            parent->getLinkCollider(parentLinkIndex);
+        parentLinkTf = parentCollider->getWorldTransform();
+      }
+      if (childLinkIndex == -1)
+      {
+        childLinkTf = child->getBaseWorldTransform();
+      }
+      else
+      {
+        btMultiBodyLinkCollider *childCollider =
+            child->getLinkCollider(childLinkIndex);
+        childLinkTf = childCollider->getWorldTransform();
+      }
+      btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf;
+      btTransform childBaseTf =  child->getBaseWorldTransform();
+      btTransform childBaseToLink =
+          childBaseTf.inverse() * childLinkTf;
+      btTransform newChildBaseTf =
+          expectedChildLinkTf * childBaseToLink.inverse();
+      child->setBaseWorldTransform(newChildBaseTf);
+    }
+  }
+
   worldInfo->world->stepSimulation(static_cast<btScalar>(this->stepSize), 1,
                                    static_cast<btScalar>(this->stepSize));
 

include/gz/physics/Joint.hh

@@ -652,6 +652,38 @@ namespace gz
             Scalar _reference) = 0;
       };
     };
+
+    /////////////////////////////////////////////////
+    class GZ_PHYSICS_VISIBLE SetFixedJointWeldChildToParentFeature
+        : public virtual Feature
+    {
+      /// \brief The Joint API for setting whether to weld a fixed joint's child
+      /// link to the parent link.
+      public: template <typename PolicyT, typename FeaturesT>
+      class Joint : public virtual Feature::Joint<PolicyT, FeaturesT>
+      {
+        /// \brief Set whether to weld the fixed joint's child link to the
+        /// parent link. If true, the child link is welded and it will move /
+        /// with the parent link as if they are part of the same body
+        /// kinematic chain. If false, the fixed joint constraint is enforced
+        /// by applying forces to both the parent and child links.
+        /// By default when a fixed joint constraint is created, this property
+        /// is set to true.
+        /// \param[in] _weldChildToParent True to weld the child link to the
+        /// parent link.
+        public: void SetWeldChildToParent(bool _weldChildToParent);
+      };
+
+      /// \private The implementation API for setting whether to weld the fixed
+      /// joint's child link to the parent link.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        // see Joint::SetWeldChildToParent above
+        public: virtual void SetFixedJointWeldChildToParent(
+            const Identity &_id, bool _weldChildToParent) = 0;
+      };
+    };
   }
 }
 

include/gz/physics/detail/Joint.hh

@@ -263,6 +263,15 @@ namespace gz
           RelativeWrench(this->GetFrameID(), this->GetTransmittedWrench()),
           _relativeTo, _inCoordinatesOf);
     }
+
+    /////////////////////////////////////////////////
+    template <typename PolicyT, typename FeaturesT>
+    void SetFixedJointWeldChildToParentFeature::Joint<PolicyT, FeaturesT>::
+    SetWeldChildToParent(bool _weldChildToParent)
+    {
+      this->template Interface<SetFixedJointWeldChildToParentFeature>()
+        ->SetFixedJointWeldChildToParent(this->identity, _weldChildToParent);
+    }
   }
 }
 

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