diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index b663c5ecf..7662a6476 100644 --- 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 index 35c8fd178..8a7d2f155 100644 --- 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