Skip to content

Commit 5c04150

Browse files
rjoomenrjoomen
authored
Use per object max margin for the broadphase and the actual link pair margin for the narrow phase, instead of the overall max margin everywhere (#1198)
1 parent 53ad1b4 commit 5c04150

File tree

11 files changed

+518
-107
lines changed

11 files changed

+518
-107
lines changed

‎tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h‎

Lines changed: 10 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -118,7 +118,9 @@ class CollisionObjectWrapper : public btCollisionObject
118118
const tesseract_common::VectorIsometry3d& getCollisionGeometriesTransforms() const;
119119

120120
/**
121-
* @brief Get the collision objects axis aligned bounding box
121+
* @brief Get the collision object's axis aligned bounding box
122+
* This AABB is extended by half of the contact processing threshold, so the broadphase takes
123+
* the full contact processing threshold into account when testing for the overlap of two AABBs.
122124
* @param aabb_min The minimum point
123125
* @param aabb_max The maximum point
124126
*/
@@ -219,11 +221,6 @@ bool needsCollisionCheck(const COW& cow1,
219221
const std::shared_ptr<const tesseract_common::ContactAllowedValidator>& validator,
220222
bool verbose = false);
221223

222-
btScalar addDiscreteSingleResult(btManifoldPoint& cp,
223-
const btCollisionObjectWrapper* colObj0Wrap,
224-
const btCollisionObjectWrapper* colObj1Wrap,
225-
ContactTestData& collisions);
226-
227224
/**
228225
* @brief Calculate the continuous contact data for casted collision shape
229226
* @param col Contact results
@@ -240,18 +237,11 @@ void calculateContinuousData(ContactResult* col,
240237
const btTransform& link_tf_inv,
241238
size_t link_index);
242239

243-
btScalar addCastSingleResult(btManifoldPoint& cp,
244-
const btCollisionObjectWrapper* colObj0Wrap,
245-
int index0,
246-
const btCollisionObjectWrapper* colObj1Wrap,
247-
int index1,
248-
ContactTestData& collisions);
249-
250240
/** @brief This is copied directly out of BulletWorld */
251241
struct TesseractBridgedManifoldResult : public btManifoldResult
252242
{
253-
btCollisionWorld::ContactResultCallback&
254-
m_resultCallback; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
243+
// NOLINTNEXTLINE(cppcoreguidelines-avoid-const-or-ref-data-members)
244+
btCollisionWorld::ContactResultCallback& m_resultCallback;
255245

256246
TesseractBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap,
257247
const btCollisionObjectWrapper* obj1Wrap,
@@ -264,10 +254,9 @@ struct TesseractBridgedManifoldResult : public btManifoldResult
264254
struct BroadphaseContactResultCallback
265255
{
266256
ContactTestData& collisions_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
267-
double contact_distance_;
268257
bool verbose_;
269258

270-
BroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false);
259+
BroadphaseContactResultCallback(ContactTestData& collisions, bool verbose = false);
271260

272261
virtual ~BroadphaseContactResultCallback() = default;
273262
BroadphaseContactResultCallback(const BroadphaseContactResultCallback&) = default;
@@ -288,7 +277,7 @@ struct BroadphaseContactResultCallback
288277

289278
struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultCallback
290279
{
291-
DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false);
280+
DiscreteBroadphaseContactResultCallback(ContactTestData& collisions, bool verbose = false);
292281

293282
btScalar addSingleResult(btManifoldPoint& cp,
294283
const btCollisionObjectWrapper* colObj0Wrap,
@@ -301,7 +290,7 @@ struct DiscreteBroadphaseContactResultCallback : public BroadphaseContactResultC
301290

302291
struct CastBroadphaseContactResultCallback : public BroadphaseContactResultCallback
303292
{
304-
CastBroadphaseContactResultCallback(ContactTestData& collisions, double contact_distance, bool verbose = false);
293+
CastBroadphaseContactResultCallback(ContactTestData& collisions, bool verbose = false);
305294

306295
btScalar addSingleResult(btManifoldPoint& cp,
307296
const btCollisionObjectWrapper* colObj0Wrap,
@@ -385,13 +374,9 @@ struct DiscreteCollisionCollector : public btCollisionWorld::ContactResultCallba
385374
{
386375
ContactTestData& collisions_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
387376
const COW::Ptr cow_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
388-
double contact_distance_;
389377
bool verbose_;
390378

391-
DiscreteCollisionCollector(ContactTestData& collisions,
392-
COW::Ptr cow,
393-
btScalar contact_distance,
394-
bool verbose = false);
379+
DiscreteCollisionCollector(ContactTestData& collisions, COW::Ptr cow, bool verbose = false);
395380

396381
btScalar addSingleResult(btManifoldPoint& cp,
397382
const btCollisionObjectWrapper* colObj0Wrap,
@@ -408,10 +393,9 @@ struct CastCollisionCollector : public btCollisionWorld::ContactResultCallback
408393
{
409394
ContactTestData& collisions_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
410395
const COW::Ptr cow_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members)
411-
double contact_distance_;
412396
bool verbose_;
413397

414-
CastCollisionCollector(ContactTestData& collisions, COW::Ptr cow, double contact_distance, bool verbose = false);
398+
CastCollisionCollector(ContactTestData& collisions, COW::Ptr cow, bool verbose = false);
415399

416400
btScalar addSingleResult(btManifoldPoint& cp,
417401
const btCollisionObjectWrapper* colObj0Wrap,

‎tesseract_collision/bullet/src/bullet_cast_bvh_manager.cpp‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -88,8 +88,6 @@ ContinuousContactManager::UPtr BulletCastBVHManager::clone() const
8888
{
8989
auto manager = std::make_unique<BulletCastBVHManager>(name_, config_info_.clone());
9090

91-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
92-
9391
for (const auto& cow : link2cow_)
9492
{
9593
COW::Ptr new_cow = cow.second->clone();
@@ -98,6 +96,8 @@ ContinuousContactManager::UPtr BulletCastBVHManager::clone() const
9896
assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
9997

10098
new_cow->setWorldTransform(cow.second->getWorldTransform());
99+
auto margin =
100+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
101101
new_cow->setContactProcessingThreshold(margin);
102102

103103
manager->addCollisionObject(new_cow);
@@ -122,7 +122,8 @@ bool BulletCastBVHManager::addCollisionObject(const std::string& name,
122122
COW::Ptr new_cow = createCollisionObject(name, mask_id, shapes, shape_poses, enabled);
123123
if (new_cow != nullptr)
124124
{
125-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
125+
auto margin =
126+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
126127
new_cow->setContactProcessingThreshold(margin);
127128
addCollisionObject(new_cow);
128129
return true;
@@ -485,8 +486,7 @@ void BulletCastBVHManager::contactTest(ContactResultMap& collisions, const Conta
485486

486487
btOverlappingPairCache* pairCache = broadphase_->getOverlappingPairCache();
487488

488-
CastBroadphaseContactResultCallback cc(contact_test_data_,
489-
contact_test_data_.collision_margin_data.getMaxCollisionMargin());
489+
CastBroadphaseContactResultCallback cc(contact_test_data_);
490490

491491
TesseractCollisionPairCallback collisionCallback(dispatch_info_, dispatcher_.get(), cc);
492492

@@ -523,10 +523,10 @@ void BulletCastBVHManager::addCollisionObject(const COW::Ptr& cow)
523523

524524
void BulletCastBVHManager::onCollisionMarginDataChanged()
525525
{
526-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
527526
for (auto& co : link2cow_)
528527
{
529528
COW::Ptr& cow = co.second;
529+
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(cow->getName()));
530530
cow->setContactProcessingThreshold(margin);
531531
if (cow->getBroadphaseHandle() != nullptr)
532532
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
@@ -535,6 +535,7 @@ void BulletCastBVHManager::onCollisionMarginDataChanged()
535535
for (auto& co : link2castcow_)
536536
{
537537
COW::Ptr& cow = co.second;
538+
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(cow->getName()));
538539
cow->setContactProcessingThreshold(margin);
539540
if (cow->getBroadphaseHandle() != nullptr)
540541
updateBroadphaseAABB(cow, broadphase_, dispatcher_);

‎tesseract_collision/bullet/src/bullet_cast_simple_manager.cpp‎

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,6 @@ ContinuousContactManager::UPtr BulletCastSimpleManager::clone() const
6868
{
6969
auto manager = std::make_unique<BulletCastSimpleManager>(name_, config_info_.clone());
7070

71-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
72-
7371
for (const auto& cow : link2cow_)
7472
{
7573
COW::Ptr new_cow = cow.second->clone();
@@ -78,6 +76,8 @@ ContinuousContactManager::UPtr BulletCastSimpleManager::clone() const
7876
assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
7977

8078
new_cow->setWorldTransform(cow.second->getWorldTransform());
79+
auto margin =
80+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
8181
new_cow->setContactProcessingThreshold(margin);
8282

8383
manager->addCollisionObject(new_cow);
@@ -102,7 +102,8 @@ bool BulletCastSimpleManager::addCollisionObject(const std::string& name,
102102
COW::Ptr new_cow = createCollisionObject(name, mask_id, shapes, shape_poses, enabled);
103103
if (new_cow != nullptr)
104104
{
105-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
105+
auto margin =
106+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
106107
new_cow->setContactProcessingThreshold(margin);
107108
addCollisionObject(new_cow);
108109
return true;
@@ -407,7 +408,7 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co
407408

408409
btCollisionObjectWrapper obA(nullptr, cow1->getCollisionShape(), cow1.get(), cow1->getWorldTransform(), -1, -1);
409410

410-
CastCollisionCollector cc(contact_test_data_, cow1, static_cast<double>(cow1->getContactProcessingThreshold()));
411+
CastCollisionCollector cc(contact_test_data_, cow1);
411412
for (auto cow2_iter = cow1_iter + 1; cow2_iter != cows_.end(); cow2_iter++)
412413
{
413414
assert(!contact_test_data_.done);
@@ -433,8 +434,10 @@ void BulletCastSimpleManager::contactTest(ContactResultMap& collisions, const Co
433434
assert(algorithm != nullptr);
434435
if (algorithm != nullptr)
435436
{
437+
// Update the contact threshold to be pair specific
438+
cc.m_closestDistanceThreshold =
439+
contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName());
436440
TesseractBridgedManifoldResult contactPointResult(&obA, &obB, cc);
437-
contactPointResult.m_closestPointDistanceThreshold = cc.m_closestDistanceThreshold;
438441

439442
// discrete collision detection query
440443
algorithm->processCollision(&obA, &obB, dispatch_info_, &contactPointResult);
@@ -475,12 +478,19 @@ void BulletCastSimpleManager::addCollisionObject(const COW::Ptr& cow)
475478

476479
void BulletCastSimpleManager::onCollisionMarginDataChanged()
477480
{
478-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
479481
for (auto& co : link2cow_)
482+
{
483+
auto margin =
484+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(co.second->getName()));
480485
co.second->setContactProcessingThreshold(margin);
486+
}
481487

482488
for (auto& co : link2castcow_)
489+
{
490+
auto margin =
491+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(co.second->getName()));
483492
co.second->setContactProcessingThreshold(margin);
493+
}
484494
}
485495

486496
} // namespace tesseract_collision::tesseract_collision_bullet

‎tesseract_collision/bullet/src/bullet_discrete_bvh_manager.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -84,8 +84,6 @@ DiscreteContactManager::UPtr BulletDiscreteBVHManager::clone() const
8484
{
8585
auto manager = std::make_unique<BulletDiscreteBVHManager>(name_, config_info_.clone());
8686

87-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
88-
8987
for (const auto& cow : link2cow_)
9088
{
9189
COW::Ptr new_cow = cow.second->clone();
@@ -94,6 +92,8 @@ DiscreteContactManager::UPtr BulletDiscreteBVHManager::clone() const
9492
assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
9593

9694
new_cow->setWorldTransform(cow.second->getWorldTransform());
95+
auto margin =
96+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
9797
new_cow->setContactProcessingThreshold(margin);
9898

9999
manager->addCollisionObject(new_cow);
@@ -118,7 +118,8 @@ bool BulletDiscreteBVHManager::addCollisionObject(const std::string& name,
118118
COW::Ptr new_cow = createCollisionObject(name, mask_id, shapes, shape_poses, enabled);
119119
if (new_cow != nullptr)
120120
{
121-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
121+
auto margin =
122+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
122123
new_cow->setContactProcessingThreshold(margin);
123124
addCollisionObject(new_cow);
124125
return true;
@@ -305,8 +306,7 @@ void BulletDiscreteBVHManager::contactTest(ContactResultMap& collisions, const C
305306

306307
broadphase_->calculateOverlappingPairs(dispatcher_.get());
307308

308-
DiscreteBroadphaseContactResultCallback cc(contact_test_data_,
309-
contact_test_data_.collision_margin_data.getMaxCollisionMargin());
309+
DiscreteBroadphaseContactResultCallback cc(contact_test_data_);
310310

311311
TesseractCollisionPairCallback collisionCallback(dispatch_info_, dispatcher_.get(), cc);
312312

@@ -325,10 +325,10 @@ void BulletDiscreteBVHManager::addCollisionObject(const COW::Ptr& cow)
325325

326326
void BulletDiscreteBVHManager::onCollisionMarginDataChanged()
327327
{
328-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
329328
for (auto& co : link2cow_)
330329
{
331330
COW::Ptr& cow = co.second;
331+
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(cow->getName()));
332332
cow->setContactProcessingThreshold(margin);
333333
assert(cow->getBroadphaseHandle() != nullptr);
334334
updateBroadphaseAABB(cow, broadphase_, dispatcher_);

‎tesseract_collision/bullet/src/bullet_discrete_simple_manager.cpp‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -70,8 +70,6 @@ DiscreteContactManager::UPtr BulletDiscreteSimpleManager::clone() const
7070
{
7171
auto manager = std::make_unique<BulletDiscreteSimpleManager>(name_, config_info_.clone());
7272

73-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
74-
7573
for (const auto& cow : link2cow_)
7674
{
7775
COW::Ptr new_cow = cow.second->clone();
@@ -80,6 +78,8 @@ DiscreteContactManager::UPtr BulletDiscreteSimpleManager::clone() const
8078
assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
8179

8280
new_cow->setWorldTransform(cow.second->getWorldTransform());
81+
auto margin =
82+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
8383
new_cow->setContactProcessingThreshold(margin);
8484

8585
manager->addCollisionObject(new_cow);
@@ -104,7 +104,8 @@ bool BulletDiscreteSimpleManager::addCollisionObject(const std::string& name,
104104
COW::Ptr new_cow = createCollisionObject(name, mask_id, shapes, shape_poses, enabled);
105105
if (new_cow != nullptr)
106106
{
107-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
107+
auto margin =
108+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(new_cow->getName()));
108109
new_cow->setContactProcessingThreshold(margin);
109110
addCollisionObject(new_cow);
110111
return true;
@@ -293,7 +294,7 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons
293294

294295
btCollisionObjectWrapper obA(nullptr, cow1->getCollisionShape(), cow1.get(), cow1->getWorldTransform(), -1, -1);
295296

296-
DiscreteCollisionCollector cc(contact_test_data_, cow1, cow1->getContactProcessingThreshold());
297+
DiscreteCollisionCollector cc(contact_test_data_, cow1);
297298
for (auto cow2_iter = cow1_iter + 1; cow2_iter != cows_.end(); cow2_iter++)
298299
{
299300
assert(!contact_test_data_.done);
@@ -319,8 +320,10 @@ void BulletDiscreteSimpleManager::contactTest(ContactResultMap& collisions, cons
319320
assert(algorithm != nullptr);
320321
if (algorithm != nullptr)
321322
{
323+
// Update the contact threshold to be pair specific
324+
cc.m_closestDistanceThreshold =
325+
contact_test_data_.collision_margin_data.getCollisionMargin(cow1->getName(), cow2->getName());
322326
TesseractBridgedManifoldResult contactPointResult(&obA, &obB, cc);
323-
contactPointResult.m_closestPointDistanceThreshold = cc.m_closestDistanceThreshold;
324327

325328
// discrete collision detection query
326329
algorithm->processCollision(&obA, &obB, dispatch_info_, &contactPointResult);
@@ -354,9 +357,12 @@ void BulletDiscreteSimpleManager::addCollisionObject(const COW::Ptr& cow)
354357

355358
void BulletDiscreteSimpleManager::onCollisionMarginDataChanged()
356359
{
357-
auto margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
358360
for (auto& co : link2cow_)
361+
{
362+
auto margin =
363+
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin(co.second->getName()));
359364
co.second->setContactProcessingThreshold(margin);
365+
}
360366
}
361367

362368
} // namespace tesseract_collision::tesseract_collision_bullet

0 commit comments

Comments
 (0)