Merge pull request #669 from gazebosim/scpeters/merge_7_main_part1

Merge gz-physics7 ➡️ main: part 1

.github/workflows/package_xml.yml

@@ -0,0 +1,11 @@
+name: Validate package.xml
+
+on:
+  pull_request:
+
+jobs:
+  package-xml:
+    runs-on: ubuntu-latest
+    name: Validate package.xml
+    steps:
+      - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy

CMakeLists.txt

@@ -78,7 +78,7 @@ gz_find_package(DART
     utils
     utils-urdf
   CONFIG
-  VERSION 6.9
+  VERSION 6.10
   REQUIRED_BY dartsim
   PKGCONFIG dart
   PKGCONFIG_VER_COMPARISON >=)

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;
 }
 
@@ -69,6 +139,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity(
   if (model)
   {
     model->body->setBaseOmega(convertVec(_angularVelocity));
+    model->body->wakeUp();
   }
 }
 
@@ -82,6 +153,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity(
   if (model)
   {
     model->body->setBaseVel(convertVec(_linearVelocity));
+    model->body->wakeUp();
   }
 }
 
@@ -95,6 +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);
+        }
+      }
+    }
   }
 }