Broadphase collision check error

[j1nkun]

In this example, collision checking with positive security margin with broadphase has incorrect result.

#include <coal/broadphase/broadphase_dynamic_AABB_tree.h>
#include <coal/broadphase/default_broadphase_callbacks.h>
#include <coal/shape/geometric_shapes.h>

int main() {
  // Create a box
  auto box1 = std::make_shared<coal::Box>(0.16, 0.27, 0.37);
  auto obj1 = std::make_shared<coal::CollisionObject>(box1);
  coal::Transform3s trans1;
  trans1.rotation() << 0, -1, 0, 0, 0, 1, -1, 0, 0;
  trans1.translation() << 1.490670, 0.845416, -0.194226;
  obj1->setTransform(trans1);
  obj1->computeAABB();

  // Create another box
  auto box2 = std::make_shared<coal::Box>(5.898, 0.06, 2.69);
  auto obj2 = std::make_shared<coal::CollisionObject>(box2);
  coal::Transform3s trans2;
  trans2.rotation() << 1, 0, 0, 0, 1, 0, 0, 0, 1;
  // trans2.rotation() << 0.9998477, -0.0174524, 0.0000000, 0.0174524,
  // 0.9998477, 0.0000000, 0.0000000, 0.0000000, 1.0000000;
  trans2.translation() << 0.000000, 1.075000, 0.555000;
  obj2->setTransform(trans2);
  obj2->computeAABB();

  // Setup collision manager
  coal::DynamicAABBTreeCollisionManager manager1;
  manager1.registerObject(obj1.get());
  manager1.setup();
  coal::DynamicAABBTreeCollisionManager manager2;
  manager2.registerObject(obj2.get());
  manager2.setup();

  // Distance check using broadphase
  coal::DistanceCallBackDefault distance_callback;
  manager1.distance(&manager2, &distance_callback);
  std::cout << "Distance: " << distance_callback.data.result.min_distance
            << std::endl;

  // Collision check with broadphase
  coal::CollisionCallBackDefault collision_callback;
  collision_callback.data.request.security_margin = 0.04;
  manager1.collide(&manager2, &collision_callback);
  std::cout << "Collision with broadphase: "
            << collision_callback.data.result.isCollision() << std::endl;

  // Collision check without broadphase
  coal::CollisionRequest request;
  request.security_margin = 0.04;
  coal::CollisionResult result;
  coal::collide(obj1.get(), obj2.get(), request, result);
  std::cout << "Collision without broadphase: " << result.isCollision()
            << std::endl;

  return 0;
}

Output

Distance: 0.014584
Collision with broadphase: 0
Collision without broadphase: 1

# Rotate trans2 by 1 deg
Distance: 0.0382369
Collision with broadphase: 1
Collision without broadphase: 1

[hazidh]

Hello, I would like to ask how do you run VS, how to compile the source code,thank you

[jcarpent]

@lmontaut Could you handle this issue?

[Everest-Zhao]

This is because only the narrow phase takes the safety margin into account, whereas the broad phase does not. When using the broad phase manager, it will not perform a narrow phase collision check if there is no overlap detected between the AABBs. Consequently, if you have a rotation, it will yield accurate results, as the AABBs may overlap after the rotation. To address this issue, You must considered the safety margin in broad phase.
I have implemented a method to check for overlap between AABBs that includes the safety margin, and it has proven effective in my code. However, this approach only addresses the general case. It does not work for objects classified as GEOM_HALFSPACE, GEOM_PLANE, or GEOM_HALFSPACE.

bool collisionRecurseWithMargin(
    DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
    DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
    CollisionCallBackDefault* callback) {
  if (root1->isLeaf() && root2->isLeaf()) {
    CollisionObject* o1 = static_cast<CollisionObject*>(root1->data);
    CollisionObject* o2 = static_cast<CollisionObject*>(root2->data);
    return leafCollideWithMargin(o1, o2, callback);
  }

  if (!nodeCollideWithMargin(root1, root2, callback)) {
    return false;
  }

  if (root2->isLeaf() ||
      (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
    if (collisionRecurseWithMargin(root1->children[0], root2, callback))
      return true;
    if (collisionRecurseWithMargin(root1->children[1], root2, callback))
      return true;
  } else {
    if (collisionRecurseWithMargin(root1, root2->children[0], callback))
      return true;
    if (collisionRecurseWithMargin(root1, root2->children[1], callback))
      return true;
  }
  return false;
}

bool collisionRecurseWithMargin(
    DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
    DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
    CollisionCallBackDefault* callback) {
  if (root1->isLeaf() && root2->isLeaf()) {
    CollisionObject* o1 = static_cast<CollisionObject*>(root1->data);
    CollisionObject* o2 = static_cast<CollisionObject*>(root2->data);
    return leafCollideWithMargin(o1, o2, callback);
  }

  if (!nodeCollideWithMargin(root1, root2, callback)) {
    return false;
  }

  if (root2->isLeaf() ||
      (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
    if (collisionRecurseWithMargin(root1->children[0], root2, callback))
      return true;
    if (collisionRecurseWithMargin(root1->children[1], root2, callback))
      return true;
  } else {
    if (collisionRecurseWithMargin(root1, root2->children[0], callback))
      return true;
    if (collisionRecurseWithMargin(root1, root2->children[1], callback))
      return true;
  }
  return false;
}

 bool nodeCollideWithMargin(
    DynamicAABBTreeCollisionManager::DynamicAABBNode* node1,
    DynamicAABBTreeCollisionManager::DynamicAABBNode* node2,
    CollisionCallBackDefault* callback) {
  // This function assumes that at least node1 or node2 is not a leaf of the
  // tree with margin.
  auto request = callback->data.request;
  CoalScalar sqrDistLowerBound;

  if (node1->isLeaf()) {
    CollisionObject* o1 = static_cast<CollisionObject*>(node1->data);
    if (o1->getNodeType() == GEOM_HALFSPACE ||
        o1->getNodeType() == GEOM_PLANE) {
      if (o1->getNodeType() == GEOM_HALFSPACE) {
        const auto& halfspace =
            static_cast<const Halfspace&>(*(o1->collisionGeometryPtr()));
        return node2->bv.overlap(transform(halfspace, o1->getTransform()));
      }
      const auto& plane =
          static_cast<const Plane&>(*(o1->collisionGeometryPtr()));
      return node2->bv.overlap(transform(plane, o1->getTransform()));
    }
  }

  if (node2->isLeaf()) {
    CollisionObject* o2 = static_cast<CollisionObject*>(node2->data);
    if (o2->getNodeType() == GEOM_HALFSPACE ||
        o2->getNodeType() == GEOM_PLANE) {
      if (o2->getNodeType() == GEOM_HALFSPACE) {
        const auto& halfspace =
            static_cast<const Halfspace&>(*(o2->collisionGeometryPtr()));
        return node1->bv.overlap(transform(halfspace, o2->getTransform()));
      }
      const auto& plane =
          static_cast<const Plane&>(*(o2->collisionGeometryPtr()));
      return node1->bv.overlap(transform(plane, o2->getTransform()));
    }
  }

  return node1->bv.overlap(node2->bv, request, sqrDistLowerBound);
}

[j1nkun]

@Everest-Zhao Thanks for the information. Your code works for us since we don't have half space/plane. I have also attached a minimal example here (using hpp-fcl).

#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
#include <hpp/fcl/broadphase/default_broadphase_callbacks.h>
#include <hpp/fcl/shape/geometric_shapes.h>

bool LeafCollideWithMargin(hpp::fcl::CollisionObject* o1,
                           hpp::fcl::CollisionObject* o2,
                           hpp::fcl::CollisionCallBackDefault* callback) {
  auto request = callback->data.request;
  double sqr_dist_lower_bound;
  bool overlap =
      o1->getAABB().overlap(o2->getAABB(), request, sqr_dist_lower_bound);
  if (overlap) {
    return (*callback)(o1, o2);
  }
  return false;
}

bool NodeCollideWithMargin(
    hpp::fcl::DynamicAABBTreeCollisionManager::DynamicAABBNode* node1,
    hpp::fcl::DynamicAABBTreeCollisionManager::DynamicAABBNode* node2,
    hpp::fcl::CollisionCallBackDefault* callback) {
  auto request = callback->data.request;
  double sqr_dist_lower_bound;
  return node1->bv.overlap(node2->bv, request, sqr_dist_lower_bound);
}

bool CollisionRecurseWithMargin(
    hpp::fcl::DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
    hpp::fcl::DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
    hpp::fcl::CollisionCallBackDefault* callback) {
  if (root1->isLeaf() && root2->isLeaf()) {
    hpp::fcl::CollisionObject* o1 =
        static_cast<hpp::fcl::CollisionObject*>(root1->data);
    hpp::fcl::CollisionObject* o2 =
        static_cast<hpp::fcl::CollisionObject*>(root2->data);
    return LeafCollideWithMargin(o1, o2, callback);
  }

  if (!NodeCollideWithMargin(root1, root2, callback)) {
    return false;
  }

  if (root2->isLeaf() ||
      (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
    if (CollisionRecurseWithMargin(root1->children[0], root2, callback))
      return true;
    if (CollisionRecurseWithMargin(root1->children[1], root2, callback))
      return true;
  } else {
    if (CollisionRecurseWithMargin(root1, root2->children[0], callback))
      return true;
    if (CollisionRecurseWithMargin(root1, root2->children[1], callback))
      return true;
  }
  return false;
}

int main() {
  // Create a sphere
  auto geom1 = std::make_shared<hpp::fcl::Sphere>(1.0);
  auto obj1 = std::make_shared<hpp::fcl::CollisionObject>(geom1);
  hpp::fcl::Transform3f trans1;
  trans1.rotation() = Eigen::Matrix3d::Identity();
  trans1.translation() << 0.0, 0.0, 0.0;
  obj1->setTransform(trans1);
  obj1->computeAABB();

  // Creat another sphere
  auto geom2 = std::make_shared<hpp::fcl::Sphere>(1.0);
  auto obj2 = std::make_shared<hpp::fcl::CollisionObject>(geom2);
  hpp::fcl::Transform3f trans2;
  trans2.rotation() = Eigen::Matrix3d::Identity();
  trans2.translation() << 2.01, 0.0, 0.0;
  obj2->setTransform(trans2);
  obj2->computeAABB();

  // Setup collision manager
  hpp::fcl::DynamicAABBTreeCollisionManager manager1;
  manager1.registerObject(obj1.get());
  manager1.setup();
  hpp::fcl::DynamicAABBTreeCollisionManager manager2;
  manager2.registerObject(obj2.get());
  manager2.setup();

  // Distance check using broadphase
  hpp::fcl::DistanceCallBackDefault distance_callback;
  manager1.distance(&manager2, &distance_callback);
  std::cout << "Distance: " << distance_callback.data.result.min_distance
            << std::endl;

  // Collision check with broadphase
  hpp::fcl::CollisionCallBackDefault collision_callback;
  collision_callback.data.request.security_margin = 0.04;
  manager1.collide(&manager2, &collision_callback);
  std::cout << "Collision with broadphase: "
            << collision_callback.data.result.isCollision() << std::endl;

  CollisionRecurseWithMargin(manager1.getTree().getRoot(),
                             manager2.getTree().getRoot(), &collision_callback);
  std::cout << "Collision with margin: "
            << collision_callback.data.result.isCollision() << std::endl;

  // Collision check without broadphase
  hpp::fcl::CollisionRequest request;
  request.security_margin = 0.04;
  hpp::fcl::CollisionResult result;
  hpp::fcl::collide(obj1.get(), obj2.get(), request, result);
  std::cout << "Collision without broadphase: " << result.isCollision()
            << std::endl;

  return 0;
}

[hazidh]

Is safety margin integrated in broad phase coal library at present?