Skip to content

Check pair margin AABB before narrow phase check #1208

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Check pair margin AABB before narrow phase check #1208

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
10 changes: 6 additions & 4 deletions tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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]) &&
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use Bullet function TestAabbAgainstAabb2(min_aabb[0], max_aabb[0], min_aabb[1], max_aabb[1])

(min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) &&
Expand All @@ -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
Expand Down
9 changes: 5 additions & 4 deletions tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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]) &&
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use Bullet function TestAabbAgainstAabb2(min_aabb[0], max_aabb[0], min_aabb[1], max_aabb[1])

(min_aabb[0][1] <= max_aabb[1][1] && max_aabb[0][1] >= min_aabb[1][1]) &&
Expand All @@ -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
Expand Down
62 changes: 41 additions & 21 deletions tesseract_collision/bullet/src/bullet_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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> CollisionObjectWrapper::clone()
Expand Down Expand Up @@ -1131,27 +1136,42 @@ bool TesseractCollisionPairCallback::processOverlap(btBroadphasePair& pair)
const auto* cow0 = static_cast<const CollisionObjectWrapper*>(pair.m_pProxy0->m_clientObject);
const auto* cow1 = static_cast<const CollisionObjectWrapper*>(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))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We're basically replicating the broadphase AABB check here, but with the actual margin between the objects. In the scenario where all margins are the same, this would lead to completely superfluous duplicate AABB checking. When margins vary, the cost of duplicate AABB checks is most likely compensated for by the reduced number of narrowphase collide calls. I just looked into the Bullet source, and maybe it is possible to override the original broadphase AABB check. Then we could insert the actual pair margin there, which would be ideal. I don't have time to dive into this right now, but I can look into it next week.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe this callback.

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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using no and full threshold instead of half twice would save one set of expansion calculations, but we'd need a getAABB_noexpand() function without expansion.


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]) &&
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use Bullet function TestAabbAgainstAabb2(min_aabb[0], max_aabb[0], min_aabb[1], max_aabb[1])

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We could add an additional check here: if (contact_threshold < (cow0->getContactProcessingThreshold() + cow1->getContactProcessingThreshold()) then do the extra aabb_check.

(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?
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, it should not return true here. We might return true if needsCollision is false above, see Bullet source, where it mentions that returning true allows removal of the pair.

return false;
}

Expand Down
10 changes: 10 additions & 0 deletions tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,13 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject<double>
*/
double getContactDistanceThreshold() const;

/**
* @brief Get AABB with provided margin
* @param margin The margin to apply to AABB
* @return The AABB
*/
fcl::AABB<double> getAABB(double margin) const;

/**
* @brief Update the internal AABB. This must be called instead of the base class computeAABB().
*
Expand All @@ -81,6 +88,9 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject<double>

/** @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<double> aabb_without_margin_;
};

} // namespace tesseract_collision::tesseract_collision_fcl
Expand Down
25 changes: 18 additions & 7 deletions tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,22 +36,33 @@ void FCLCollisionObjectWrapper::setContactDistanceThreshold(double contact_dista

double FCLCollisionObjectWrapper::getContactDistanceThreshold() const { return contact_distance_; }

fcl::AABB<double> FCLCollisionObjectWrapper::getAABB(double margin) const
{
fcl::AABB<double> aabb_with_margin{ aabb_without_margin_ };
fcl::Vector3<double> delta = fcl::Vector3<double>::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<double> delta = fcl::Vector3<double>::Constant(contact_distance_ / 2.0);
aabb.min_ -= delta;
aabb.max_ += delta;
aabb_without_margin_ = translate(cgeom->aabb_local, t.translation());
}
else
{
fcl::Vector3<double> center = t * cgeom->aabb_center;
fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(cgeom->aabb_radius + (contact_distance_ / 2.0));
aabb.min_ = center - delta;
aabb.max_ = center + delta;
fcl::Vector3<double> aabb_radius = fcl::Vector3<double>::Constant(cgeom->aabb_radius);
aabb_without_margin_.min_ = center - aabb_radius;
aabb_without_margin_.max_ = center + aabb_radius;
}

aabb = aabb_without_margin_;
fcl::Vector3<double> delta = fcl::Vector3<double>::Constant(contact_distance_ / 2.0);
aabb.min_ -= delta;
aabb.max_ += delta;
}

void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; }
Expand Down
29 changes: 28 additions & 1 deletion tesseract_collision/fcl/src/fcl_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> aabb1 = static_cast<const FCLCollisionObjectWrapper*>(o1)->getAABB(margin);
fcl::AABB<double> aabb2 = static_cast<const FCLCollisionObjectWrapper*>(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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can add the same check as suggested above: first check if the sum of the object margins is smaller than the pair margin before doing the extra aabb check.

return false;

std::size_t num_contacts = (cdata->req.contact_limit > 0) ? static_cast<std::size_t>(cdata->req.contact_limit) :
std::numeric_limits<std::size_t>::max();
if (cdata->req.type == ContactTestType::FIRST)
Expand Down Expand Up @@ -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<double> aabb1 = static_cast<const FCLCollisionObjectWrapper*>(o1)->getAABB(contact_threshold / 2.0);
fcl::AABB<double> aabb2 = static_cast<const FCLCollisionObjectWrapper*>(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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can add the same check as suggested above: first check if the sum of the object margins is smaller than the pair margin before doing the extra 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();
Expand Down
Loading