Coal Collision Detection Library

Coal is an extension of the Flexible Collision Library (FCL) with several performance and functional improvements:

• High-Performance Algorithms: Uses dedicated GJK and EPA implementations (not relying on libccd) and an accelerated version of collision detection à la Nesterov.
• Safety Margins: Supports security margins in collision detection. A positive margin can prevent shapes from getting too close (useful for motion planning), while a negative margin can stabilize physics simulations.
• Advanced Geometry Support: Supports height fields, capsules, ellipsoids, cones, planes, halfspaces, and convex meshes.
• Contact Information: Efficiently computes contact points and contact patches.
• Distance Bounds: Computes a lower bound of the distance between objects when no collision is found.
• Serialization: Full support for object serialization via Boost.Serialization.
• Python Bindings: Provides Python bindings for easy prototyping.

You can perform collision detection in Python using the coal bindings. The typical workflow involves creating shapes (like Ellipsoid or loaded meshes), defining their spatial placement using Transform3s, and then using coal.collide with a CollisionRequest and CollisionResult object.

Key steps:

1. Load Meshes: Use coal.MeshLoader() to load geometry files.
2. Define Shapes: Instantiate shape objects like coal.Ellipsoid or use the convex hull from a loaded mesh.
3. Set Placements: Use coal.Transform3s to define translation and rotation. These can be populated using numpy arrays or pinocchio SE3 modules.
4. Execute Collision: Call coal.collide(shape1, T1, shape2, T2, col_req, col_res).
5. Retrieve Results: Check col_res.isCollision() and use col_res.getContact(index) to access contact details like penetration_depth, normal, and witness points.
6. Cleanup: Always call col_res.clear() before reusing a CollisionResult object for a new collision call to avoid stale data.

import numpy as np
import coal
import pinocchio as pin

def loadConvexMesh(file_name: str):
    loader = coal.MeshLoader()
    bvh: coal.BVHModelBase = loader.load(file_name)
    bvh.buildConvexHull(True, "Qt")
    return bvh.convex

if __name__ == "__main__":
    # Create coal shapes
    shape1 = coal.Ellipsoid(0.7, 1.0, 0.8)
    shape2 = loadConvexMesh("../path/to/mesh/file.obj")

    # Define the shapes' placement in 3D space
    T1 = coal.Transform3s()
    T1.setTranslation(pin.SE3.Random().translation)
    T1.setRotation(pin.SE3.Random().rotation)
    T2 = coal.Transform3s()
    T1.setTranslation(np.random.rand(3))
    T2.setRotation(pin.SE3.Random().rotation)

    # Define collision requests and results
    col_req = coal.CollisionRequest()
    col_res = coal.CollisionResult()

    # Collision call
    coal.collide(shape1, T1, shape2, T2, col_req, col_res)

    # Accessing the collision result once it has been populated
    print("Is collision? ", {col_res.isCollision()})
    if col_res.isCollision():
        contact: coal.Contact = col_res.getContact(0)
        print("Penetration depth: ", contact.penetration_depth)
        print("Distance between the shapes including the security margin: ", contact.penetration_depth + col_req.security_margin)
        print("Witness point shape1: ", contact.getNearestPoint1())
        print("Witness point shape2: ", contact.getNearestPoint2())
        print("Normal: ", contact.normal)

    # Before running another collision call, it is important to clear the old one
    col_res.clear()

To check for collisions between two shapes in C++, use the coal::collide function. You must provide the shapes, their respective transformations (coal::Transform3s), a coal::CollisionRequest to configure parameters like security margins, and a coal::CollisionResult to store the output.

Key features of the collision result include:

• isCollision(): Returns true if a collision is detected.
• getContact(index): Retrieves contact information such as penetration_depth, nearest_points, and the collision normal.
• clear(): Always call col_res.clear() before reusing a CollisionResult object for a new test to ensure old data is removed.

#include "coal/math/transform.h"
#include "coal/mesh_loader/loader.h"
#include "coal/BVH/BVH_model.h"
#include "coal/collision.h"
#include "coal/collision_data.h"
#include <iostream>
#include <memory>

// Function to load a convex mesh from a `.obj`, `.stl` or `.dae` file.
std::shared_ptr<coal::ConvexBase> loadConvexMesh(const std::string& file_name) {
  coal::NODE_TYPE bv_type = coal::BV_AABB;
  coal::MeshLoader loader(bv_type);
  coal::BVHModelPtr_t bvh = loader.load(file_name);
  bvh->buildConvexHull(true, "Qt");
  return bvh->convex;
}

int main() {
  std::shared_ptr<coal::Ellipsoid> shape1 = std::make_shared<coal::Ellipsoid>(0.7, 1.0, 0.8);
  std::shared_ptr<coal::ConvexBase> shape2 = loadConvexMesh("../path/to/mesh/file.obj");

  coal::Transform3s T1;
  T1.setQuatRotation(coal::Quaternion3f::UnitRandom());
  T1.setTranslation(coal::Vec3s::Random());
  coal::Transform3s T2 = coal::Transform3s::Identity();
  T2.setQuatRotation(coal::Quaternion3f::UnitRandom());
  T2.setTranslation(coal::Vec3s::Random());

  coal::CollisionRequest col_req;
  col_req.security_margin = 1e-1;
  coal::CollisionResult col_res;

  coal::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res);

  std::cout << "Collision? " << col_res.isCollision() << "\n";
  if (col_res.isCollision()) {
    coal::Contact contact = col_res.getContact(0);
    std::cout << "Penetration depth: " << contact.penetration_depth << "\n";
    std::cout << "Distance between the shapes including the security margin: " << contact.penetration_depth + col_req.security_margin << "\n";
    std::cout << "Witness point on shape1: " << contact.nearest_points[0].transpose() << "\n";
    std::cout << "Witness point on shape2: " << contact.nearest_points[1].transpose() << "\n";
    std::cout << "Normal: " << contact.normal.transpose() << "\n";
  }

  col_res.clear();

  return 0;
}