further optimize collision flags

Signed-off-by: Ian Chen <[email protected]>
gazebosim · May 10, 2024 · ab22dfc · ab22dfc
1 parent 33af03e
commit ab22dfc
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 10 deletions.
diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc
@@ -360,6 +360,13 @@ Identity JointFeatures::AttachFixedJoint(
           childProxy->m_collisionFilterMask =
               btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
         }
+#if BT_BULLET_VERSION >= 307
+        childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
+#else
+	int oldFlags = linkInfo->collider->getCollisionFlags();
+	linkInfo->collider->setCollisionFlags(oldFlags |
+            btCollisionObject::CF_STATIC_OBJECT);
+#endif
       }
     }
 
@@ -401,6 +408,16 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
             childProxy->m_collisionFilterGroup =
                 btBroadphaseProxy::DefaultFilter;
             childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
+
+#if BT_BULLET_VERSION >= 307
+            childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
+#else
+            int oldFlags = linkInfo->collider->getCollisionFlags();
+            oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT |
+                          btCollisionObject::CF_KINEMATIC_OBJECT);
+            linkInfo->collider->setCollisionFlags(oldFlags |
+                btCollisionObject::CF_DYNAMIC_OBJECT);
+#endif
           }
         }
       }

diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc
@@ -1239,11 +1239,8 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld)
                   frameDataModel1Body.pose.translation().z(), 1e-3);
       EXPECT_NEAR(initialModel2Pose.Pos().Z(),
                   frameDataModel2Body.pose.translation().z(), 1e-3);
-      // \todo(iche033) Tolerance increased for bullet-featherstone
-      // as it tries to resolve overlapping bodies held together by
-      // a fixed joint
       EXPECT_NEAR(initialModel3Pose.Pos().Z(),
-                  frameDataModel3Body.pose.translation().z(), 1e-2);
+                  frameDataModel3Body.pose.translation().z(), 1e-3);
 
       // Expect all models to have zero velocities
       gz::math::Vector3d body1LinearVelocity =
@@ -1254,12 +1251,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetachFixedToWorld)
         gz::math::eigen3::convert(frameDataModel3Body.linearVelocity);
       EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-3);
       EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-3);
-      // \todo(iche033) bullet-featherstone produces non-zero velocities
-      // for overlapping bodies
-      if (this->PhysicsEngineName(name) != "bullet-featherstone")
-      {
-        EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3);
-      }
+      EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-3);
     }
 
     // now detach joint and expect model2 and model3 to start moving