fix test on jammy

Signed-off-by: Ian Chen <ichen@openrobotics.org>
gazebosim · Jul 9, 2024 · f1bd5b9 · f1bd5b9
1 parent bbcd8a5
commit f1bd5b9
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
@@ -1294,9 +1294,9 @@ bool SDFFeatures::AddSdfCollision(
 
     if (!linkInfo->collider)
     {
-      this->CreateLinkCollider(_linkID, _isStatic);
+      this->CreateLinkCollider(_linkID, _isStatic, shape.get(),
+          btInertialToCollision);
 
-      linkInfo->shape->addChildShape(btInertialToCollision, shape.get());
       linkInfo->collider->setRestitution(static_cast<btScalar>(restitution));
       linkInfo->collider->setRollingFriction(
         static_cast<btScalar>(rollingFriction));
@@ -1437,7 +1437,8 @@ Identity SDFFeatures::ConstructSdfJoint(
 }
 
 /////////////////////////////////////////////////
-void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic)
+void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic,
+    btCollisionShape *_shape, const btTransform &_shapeTF)
 {
   auto *linkInfo = this->ReferenceInterface<LinkInfo>(_linkID);
   auto *modelInfo = this->ReferenceInterface<ModelInfo>(linkInfo->model);
@@ -1450,6 +1451,10 @@ void SDFFeatures::CreateLinkCollider(const Identity &_linkID, bool _isStatic)
     modelInfo->body.get(), linkIndexInModel);
 
   linkInfo->shape = std::make_unique<btCompoundShape>();
+
+  if (_shape)
+    linkInfo->shape->addChildShape(_shapeTF, _shape);
+
   linkInfo->collider->setCollisionShape(linkInfo->shape.get());
 
   if (linkIndexInModel >= 0)

diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh
@@ -84,7 +84,8 @@ class SDFFeatures :
   /// \param[in] _isStatic True if the link is static
   /// \param[in] _shape Collision shape to attach to link
   private: void CreateLinkCollider(const Identity &_linkID,
-      bool _isStatic);
+      bool _isStatic, btCollisionShape *_shape = nullptr,
+      const btTransform &_shapeTF = btTransform::getIdentity());
 };
 
 }  // namespace bullet_featherstone