diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h index 8a630a52573..92820c5aa56 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h @@ -126,6 +126,15 @@ class CollisionObjectWrapper : public btCollisionObject */ void getAABB(btVector3& aabb_min, btVector3& aabb_max) const; + /** + * @brief Get the collision object's axis aligned bounding box + * This AABB is extended by the provided margin + * @param aabb_min The minimum point + * @param aabb_max The maximum point + * @margin margin The value to extend the aabb + */ + void getAABB(btVector3& aabb_min, btVector3& aabb_max, btScalar margin) const; + /** * @brief This clones the collision objects but not the collision shape wich is const. * @return Shared Pointer to the cloned collision object diff --git a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp index a400d2379e4..d79476c6dbf 100644 --- a/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp @@ -404,7 +404,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co continue; btVector3 min_aabb[2], max_aabb[2]; // NOLINT - cow1->getAABB(min_aabb[0], max_aabb[0]); + cow1->getAABB(min_aabb[0], max_aabb[0], 0.0); btCollisionObjectWrapper obA(nullptr, cow1->getCollisionShape(), cow1.get(), cow1->getWorldTransform(), -1, -1); @@ -414,7 +414,10 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co assert(!contact_test_data_.done); const COW::Ptr& cow2 = *cow2_iter; - cow2->getAABB(min_aabb[1], max_aabb[1]); + + const double contact_threshold = + contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName()); + cow2->getAABB(min_aabb[1], max_aabb[1], contact_threshold); bool aabb_check = (min_aabb[0][0] <= max_aabb[1][0] && max_aabb[0][0] >= min_aabb[1][0]) && (min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) && @@ -435,8 +438,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co if (algorithm != nullptr) { // Update the contact threshold to be pair specific - cc.m_closestDistanceThreshold = - contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName()); + cc.m_closestDistanceThreshold = contact_threshold; TesseractBridgedManifoldResult contactPointResult(&obA, &obB, cc); // discrete collision detection query diff --git a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp index d714e1f0711..cb15473b9ff 100644 --- a/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp +++ b/tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp @@ -290,7 +290,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons continue; btVector3 min_aabb[2], max_aabb[2]; // NOLINT - cow1->getAABB(min_aabb[0], max_aabb[0]); + cow1->getAABB(min_aabb[0], max_aabb[0], 0.0); btCollisionObjectWrapper obA(nullptr, cow1->getCollisionShape(), cow1.get(), cow1->getWorldTransform(), -1, -1); @@ -300,7 +300,9 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons assert(!contact_test_data_.done); const COW::Ptr& cow2 = *cow2_iter; - cow2->getAABB(min_aabb[1], max_aabb[1]); + const double contact_threshold = + contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName()); + cow2->getAABB(min_aabb[1], max_aabb[1], contact_threshold); bool aabb_check = (min_aabb[0][0] <= max_aabb[1][0] && max_aabb[0][0] >= min_aabb[1][0]) && (min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) && @@ -321,8 +323,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons if (algorithm != nullptr) { // Update the contact threshold to be pair specific - cc.m_closestDistanceThreshold = - contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName()); + cc.m_closestDistanceThreshold = contact_threshold; TesseractBridgedManifoldResult contactPointResult(&obA, &obB, cc); // discrete collision detection query diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index 64b1ae09cd0..ab7bd332b4f 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -528,12 +528,17 @@ const tesseract_common::VectorIsometry3d& CollisionObjectWrapper::getCollisionGe } void CollisionObjectWrapper::getAABB(btVector3& aabb_min, btVector3& aabb_max) const +{ + const btScalar d = getContactProcessingThreshold() / 2.0; + getAABB(aabb_min, aabb_max, d); +} + +void CollisionObjectWrapper::getAABB(btVector3& aabb_min, btVector3& aabb_max, btScalar margin) const { getCollisionShape()->getAabb(getWorldTransform(), aabb_min, aabb_max); - const btScalar& d = getContactProcessingThreshold() / 2.0; - btVector3 contactThreshold(d, d, d); - aabb_min -= contactThreshold; - aabb_max += contactThreshold; + btVector3 contact_threshold(margin, margin, margin); + aabb_min -= contact_threshold; + aabb_max += contact_threshold; } std::shared_ptr CollisionObjectWrapper::clone() @@ -1131,27 +1136,42 @@ bool TesseractCollisionPairCallback::processOverlap(btBroadphasePair& pair) const auto* cow0 = static_cast(pair.m_pProxy0->m_clientObject); const auto* cow1 = static_cast(pair.m_pProxy1->m_clientObject); - if (results_callback_.needsCollision(cow0, cow1)) - { - btCollisionObjectWrapper obj0Wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); - btCollisionObjectWrapper obj1Wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); + if (!results_callback_.needsCollision(cow0, cow1)) + return false; - // dispatcher will keep algorithms persistent in the collision pair - if (pair.m_algorithm == nullptr) - { - pair.m_algorithm = dispatcher_->findAlgorithm(&obj0Wrap, &obj1Wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS); - } + const double contact_threshold = + results_callback_.collisions_.collision_margin_data.getCollisionMargin(cow0->getName(), cow1->getName()); + btVector3 min_aabb[2], max_aabb[2]; // NOLINT + cow0->getAABB(min_aabb[0], max_aabb[0], contact_threshold / 2.0); + cow1->getAABB(min_aabb[1], max_aabb[1], contact_threshold / 2.0); - if (pair.m_algorithm != nullptr) - { - TesseractBroadphaseBridgedManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap, results_callback_); - contactPointResult.m_closestPointDistanceThreshold = - results_callback_.collisions_.collision_margin_data.getCollisionMargin(cow0->getName(), cow1->getName()); + const bool aabb_check = (min_aabb[0][0] <= max_aabb[1][0] && max_aabb[0][0] >= min_aabb[1][0]) && + (min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) && + (min_aabb[0][2] <= max_aabb[1][2] && max_aabb[0][2] >= min_aabb[1][2]); - // discrete collision detection query - pair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatch_info_, &contactPointResult); - } + // Check if ABB are overlapping + if (!aabb_check) + return false; + + btCollisionObjectWrapper obj0Wrap(nullptr, cow0->getCollisionShape(), cow0, cow0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1Wrap(nullptr, cow1->getCollisionShape(), cow1, cow1->getWorldTransform(), -1, -1); + + // dispatcher will keep algorithms persistent in the collision pair + if (pair.m_algorithm == nullptr) + { + pair.m_algorithm = dispatcher_->findAlgorithm(&obj0Wrap, &obj1Wrap, nullptr, BT_CLOSEST_POINT_ALGORITHMS); } + + if (pair.m_algorithm != nullptr) + { + TesseractBroadphaseBridgedManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap, results_callback_); + contactPointResult.m_closestPointDistanceThreshold = contact_threshold; + + // discrete collision detection query + pair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatch_info_, &contactPointResult); + } + + // Should this return true? return false; } diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h index 3d901c9e105..ce1bc324081 100644 --- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h @@ -57,6 +57,13 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject */ double getContactDistanceThreshold() const; + /** + * @brief Get AABB with provided margin + * @param margin The margin to apply to AABB + * @return The AABB + */ + fcl::AABB getAABB(double margin) const; + /** * @brief Update the internal AABB. This must be called instead of the base class computeAABB(). * @@ -81,6 +88,9 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject /** @brief The shape index, which is the geometries index in the urdf. */ int shape_index_{ -1 }; + + /** @brief AABB which does not include margin */ + fcl::AABB aabb_without_margin_; }; } // namespace tesseract_collision::tesseract_collision_fcl diff --git a/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp b/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp index 3c7607ac508..1b5f32beaa4 100644 --- a/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp +++ b/tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp @@ -36,22 +36,33 @@ void FCLCollisionObjectWrapper::setContactDistanceThreshold(double contact_dista double FCLCollisionObjectWrapper::getContactDistanceThreshold() const { return contact_distance_; } +fcl::AABB FCLCollisionObjectWrapper::getAABB(double margin) const +{ + fcl::AABB aabb_with_margin{ aabb_without_margin_ }; + fcl::Vector3 delta = fcl::Vector3::Constant(margin); + aabb_with_margin.min_ -= delta; + aabb_with_margin.max_ += delta; + return aabb_with_margin; +} + void FCLCollisionObjectWrapper::updateAABB() { if (t.linear().isIdentity()) { - aabb = translate(cgeom->aabb_local, t.translation()); - fcl::Vector3 delta = fcl::Vector3::Constant(contact_distance_ / 2.0); - aabb.min_ -= delta; - aabb.max_ += delta; + aabb_without_margin_ = translate(cgeom->aabb_local, t.translation()); } else { fcl::Vector3 center = t * cgeom->aabb_center; - fcl::Vector3 delta = fcl::Vector3::Constant(cgeom->aabb_radius + (contact_distance_ / 2.0)); - aabb.min_ = center - delta; - aabb.max_ = center + delta; + fcl::Vector3 aabb_radius = fcl::Vector3::Constant(cgeom->aabb_radius); + aabb_without_margin_.min_ = center - aabb_radius; + aabb_without_margin_.max_ = center + aabb_radius; } + + aabb = aabb_without_margin_; + fcl::Vector3 delta = fcl::Vector3::Constant(contact_distance_ / 2.0); + aabb.min_ -= delta; + aabb.max_ += delta; } void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; } diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index 7211187bd94..028803985ab 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -237,6 +237,19 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi if (!needs_collision) return false; + const double margin = cdata->collision_margin_data.getCollisionMargin(cd1->getName(), cd2->getName()) / 2.0; + + fcl::AABB aabb1 = static_cast(o1)->getAABB(margin); + fcl::AABB aabb2 = static_cast(o2)->getAABB(margin); + + const bool aabb_check = (aabb1.min_[0] <= aabb2.max_[0] && aabb1.max_[0] >= aabb2.min_[0]) && + (aabb1.min_[1] <= aabb2.max_[1] && aabb1.max_[1] >= aabb2.min_[1]) && + (aabb1.min_[2] <= aabb2.max_[2] && aabb1.max_[2] >= aabb2.min_[2]); + + // Check if ABB are overlapping + if (!aabb_check) + return false; + std::size_t num_contacts = (cdata->req.contact_limit > 0) ? static_cast(cdata->req.contact_limit) : std::numeric_limits::max(); if (cdata->req.type == ContactTestType::FIRST) @@ -305,11 +318,25 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void if (!needs_collision) return false; + const double contact_threshold = + cdata->collision_margin_data.getCollisionMargin(cd1->getName(), cd2->getName()) / 2.0; + + fcl::AABB aabb1 = static_cast(o1)->getAABB(contact_threshold / 2.0); + fcl::AABB aabb2 = static_cast(o2)->getAABB(contact_threshold / 2.0); + + const bool aabb_check = (aabb1.min_[0] <= aabb2.max_[0] && aabb1.max_[0] >= aabb2.min_[0]) && + (aabb1.min_[1] <= aabb2.max_[1] && aabb1.max_[1] >= aabb2.min_[1]) && + (aabb1.min_[2] <= aabb2.max_[2] && aabb1.max_[2] >= aabb2.min_[2]); + + // Check if ABB are overlapping + if (!aabb_check) + return false; + fcl::DistanceResultd fcl_result; fcl::DistanceRequestd fcl_request(true, true); double d = fcl::distance(o1, o2, fcl_request, fcl_result); - if (d < cdata->collision_margin_data.getMaxCollisionMargin()) + if (d < contact_threshold) { const Eigen::Isometry3d& tf1 = cd1->getCollisionObjectsTransform(); const Eigen::Isometry3d& tf2 = cd2->getCollisionObjectsTransform();