Replace python-fcl with Coal for aarch64 Linux support

docs/install_notes.rst

@@ -78,19 +78,6 @@ You can test that this process has worked correctly by going back to the VerifAI
 MacOS
 -----
 
-.. _pythonfcl:
-
-Installing python-fcl on Apple silicon
-++++++++++++++++++++++++++++++++++++++
-
-If on an Apple-silicon machine you get an error related to pip being unable to install ``python-fcl``, it can be installed manually using the following steps:
-
-1. Clone the `python-fcl <https://github.com/BerkeleyAutomation/python-fcl>`_ repository.
-2. Navigate to the repository.
-3. Install dependencies using `Homebrew <https://brew.sh>`__ with the following command: :command:`brew install fcl eigen octomap`
-4. Activate your virtual environment if you haven't already.
-5. Install the package using pip with the following command: :command:`CPATH=$(brew --prefix)/include:$(brew --prefix)/include/eigen3 LD_LIBRARY_PATH=$(brew --prefix)/lib python -m pip install .`
-
 Windows
 -------
 

pyproject.toml

@@ -41,7 +41,7 @@ dependencies = [
 	'pygame >= 2.1.3.dev8, <3; python_version >= "3.11"',
 	'pygame ~= 2.0; python_version < "3.11"',
 	"pyglet >= 1.5, <= 1.5.26",
-	"python-fcl >= 0.7",
+	"coal >= 3.0",
 	"Rtree ~= 1.0",
 	"rv-ltl ~= 0.1",
 	"scikit-image ~= 0.21",

src/scenic/core/regions.py

@@ -13,7 +13,7 @@
 import random
 import warnings
 
-import fcl
+import coal
 import numpy
 import scipy
 import shapely
@@ -758,16 +758,26 @@ class UndefinedSamplingException(Exception):
 ###################################################################################################
 
 
-class SurfaceCollisionTrimesh(trimesh.Trimesh):
-    """A Trimesh object that always returns non-convex.
+def _meshBVH(mesh):
+    """Build a Coal BVH collision geometry from a trimesh mesh."""
+    bvh = coal.BVHModelOBBRSS()
+    faces = numpy.asarray(mesh.faces, dtype=numpy.int64)
+    bvh.beginModel(len(faces), len(mesh.vertices))
+    bvh.addVertices(numpy.asarray(mesh.vertices, dtype=numpy.float64))
+    bvh.addTriangles(faces)
+    bvh.endModel()
+    return bvh
 
-    Used so that fcl doesn't find collision without an actual surface
-    intersection.
-    """
 
-    @property
-    def is_convex(self):
-        return False
+def _meshesCollide(mesh_a, mesh_b):
+    """Check if two trimesh meshes have colliding surfaces using Coal."""
+    bvh_a = _meshBVH(mesh_a)
+    bvh_b = _meshBVH(mesh_b)
+    t = coal.Transform3s()
+    req = coal.CollisionRequest()
+    res = coal.CollisionResult()
+    coal.collide(bvh_a, t, bvh_b, t, req, res)
+    return res.isCollision()
 
 
 class MeshRegion(Region):
@@ -1217,21 +1227,23 @@ def intersects(self, other, triedReversed=False):
                     return False
 
             # PASS 3
-            # Use FCL to check for intersection between the surfaces.
+            # Use Coal to check for intersection between the surfaces.
             # If the surfaces collide, that implies a collision of the volumes.
             # Cheaper than computing volumes immediately.
             # (N.B. Does not require explicitly building the mesh, if we have a
             # precomputed _scaledShape available.)
 
-            selfObj = fcl.CollisionObject(*self._fclData)
-            otherObj = fcl.CollisionObject(*other._fclData)
-            surface_collision = fcl.collide(selfObj, otherObj)
+            selfObj = coal.CollisionObject(*self._collisionData)
+            otherObj = coal.CollisionObject(*other._collisionData)
+            col_req = coal.CollisionRequest()
+            col_res = coal.CollisionResult()
+            surface_collision = coal.collide(selfObj, otherObj, col_req, col_res)
 
             if surface_collision:
                 return True
 
             if self.isConvex and other.isConvex:
-                # For convex shapes, FCL detects containment as well as
+                # For convex shapes, Coal detects containment as well as
                 # surface intersections, so we can just return the result
                 return surface_collision
 
@@ -1266,22 +1278,12 @@ def intersects(self, other, triedReversed=False):
                 return False
 
             # PASS 2
-            # Use Trimesh's collision manager to check for intersection.
+            # Use Coal to check for surface intersection.
             # If the surfaces collide (or surface is contained in the mesh),
             # that implies a collision of the volumes. Cheaper than computing
-            # intersection. Must use a SurfaceCollisionTrimesh object for the surface
-            # mesh to ensure that a collision implies surfaces touching.
-            collision_manager = trimesh.collision.CollisionManager()
-
-            collision_manager.add_object("SelfRegion", self.mesh)
-            collision_manager.add_object(
-                "OtherRegion",
-                SurfaceCollisionTrimesh(
-                    faces=other.mesh.faces, vertices=other.mesh.vertices
-                ),
-            )
-
-            surface_collision = collision_manager.in_collision_internal()
+            # intersection. Always use BVH (not convex) so that only actual
+            # surface intersections are detected.
+            surface_collision = _meshesCollide(self.mesh, other.mesh)
 
             if surface_collision:
                 return True
@@ -1924,29 +1926,39 @@ def _bodyCount(self):
         return self.mesh.body_count
 
     @cached_property
-    def _fclData(self):
+    def _collisionData(self):
         # Use precomputed geometry if available
         if self._scaledShape:
-            geom = self._scaledShape._fclData[0]
-            trans = fcl.Transform(self.rotation.r.as_matrix(), numpy.array(self.position))
+            geom = self._scaledShape._collisionData[0]
+            trans = coal.Transform3s(
+                self.rotation.r.as_matrix(),
+                numpy.array(self.position),
+            )
             return geom, trans
 
         mesh = self.mesh
         if self.isConvex:
-            vertCounts = 3 * numpy.ones((len(mesh.faces), 1), dtype=numpy.int64)
-            faces = numpy.concatenate((vertCounts, mesh.faces), axis=1)
-            geom = fcl.Convex(mesh.vertices, len(faces), faces.flatten())
+            bvh = coal.BVHModelOBBRSS()
+            faces = numpy.asarray(mesh.faces, dtype=numpy.int64)
+            bvh.beginModel(len(faces), len(mesh.vertices))
+            bvh.addVertices(numpy.asarray(mesh.vertices, dtype=numpy.float64))
+            bvh.addTriangles(faces)
+            bvh.endModel()
+            bvh.buildConvexRepresentation(False)
+            geom = bvh.convex
         else:
-            geom = fcl.BVHModel()
-            geom.beginModel(num_tris_=len(mesh.faces), num_vertices_=len(mesh.vertices))
-            geom.addSubModel(mesh.vertices, mesh.faces)
+            geom = coal.BVHModelOBBRSS()
+            faces = numpy.asarray(mesh.faces, dtype=numpy.int64)
+            geom.beginModel(len(faces), len(mesh.vertices))
+            geom.addVertices(numpy.asarray(mesh.vertices, dtype=numpy.float64))
+            geom.addTriangles(faces)
             geom.endModel()
-        trans = fcl.Transform()
+        trans = coal.Transform3s()
         return geom, trans
 
     def __getstate__(self):
         state = self.__dict__.copy()
-        state.pop("_cached__fclData", None)  # remove non-picklable FCL objects
+        state.pop("_cached__collisionData", None)  # remove non-picklable Coal objects
         return state
 
 
@@ -2005,27 +2017,10 @@ def intersects(self, other, triedReversed=False):
         * `PolygonalFootprintRegion`
         """
         if isinstance(other, MeshSurfaceRegion):
-            # Uses Trimesh's collision manager to check for intersection of the
-            # surfaces. Use SurfaceCollisionTrimesh objects to ensure collisions
-            # actually imply a surface collision.
-            collision_manager = trimesh.collision.CollisionManager()
-
-            collision_manager.add_object(
-                "SelfRegion",
-                SurfaceCollisionTrimesh(
-                    faces=self.mesh.faces, vertices=self.mesh.vertices
-                ),
-            )
-            collision_manager.add_object(
-                "OtherRegion",
-                SurfaceCollisionTrimesh(
-                    faces=other.mesh.faces, vertices=other.mesh.vertices
-                ),
-            )
-
-            surface_collision = collision_manager.in_collision_internal()
-
-            return surface_collision
+            # Use Coal to check for intersection of the surfaces.
+            # Always use BVH (not convex) so that only actual surface
+            # intersections are detected.
+            return _meshesCollide(self.mesh, other.mesh)
 
         if isinstance(other, PolygonalFootprintRegion):
             # Determine the mesh's vertical bounds (adding a little extra to avoid mesh errors) and

src/scenic/core/requirements.py

@@ -6,10 +6,9 @@
 import inspect
 import itertools
 
-import fcl
+import coal
 import numpy
 import rv_ltl
-import trimesh
 
 from scenic.core.distributions import Samplable, needsSampling, toDistribution
 from scenic.core.errors import InvalidScenarioError
@@ -360,24 +359,30 @@ def __init__(self, objects, optional=True):
 
     def falsifiedByInner(self, sample):
         objects = tuple(sample[obj] for obj in self.objects)
-        manager = fcl.DynamicAABBTreeCollisionManager()
-        objForGeom = {}
+        manager = coal.DynamicAABBTreeCollisionManager()
+        geomIdToObj = {}
         for i, obj in enumerate(objects):
             if obj.allowCollisions:
                 continue
-            geom, trans = obj.occupiedSpace._fclData
-            collisionObject = fcl.CollisionObject(geom, trans)
-            objForGeom[geom] = obj
+            geom, trans = obj.occupiedSpace._collisionData
+            collisionObject = coal.CollisionObject(geom, trans)
+            # collisionGeometry().id() returns the stable C++ address of the
+            # geometry, matching contact.o1.id() / contact.o2.id() in results.
+            geomIdToObj[collisionObject.collisionGeometry().id()] = obj
             manager.registerObject(collisionObject)
 
         manager.setup()
-        cdata = fcl.CollisionData()
-        manager.collide(cdata, fcl.defaultCollisionCallback)
-        collision = cdata.result.is_collision
+        callback = coal.CollisionCallBackDefault()
+        callback.data.request.num_max_contacts = 1
+        manager.collide(callback)
+        collision = callback.data.result.isCollision()
 
         if collision:
-            contact = cdata.result.contacts[0]
-            self._collidingObjects = (objForGeom[contact.o1], objForGeom[contact.o2])
+            contact = callback.data.result.getContact(0)
+            self._collidingObjects = (
+                geomIdToObj[contact.o1.id()],
+                geomIdToObj[contact.o2.id()],
+            )
 
         return collision
 

tests/core/test_regions.py

@@ -1,7 +1,7 @@
 import math
 from pathlib import Path
 
-import fcl
+import coal
 import pytest
 import shapely.geometry
 import trimesh.voxel
@@ -481,31 +481,35 @@ def test_mesh_interiorPoint():
         assert SpheroidRegion(dimensions=(d, d, d), position=cp).containsRegion(reg)
 
 
-def test_mesh_fcl():
-    """Test internal construction of FCL models for MeshVolumeRegions."""
+def test_mesh_collision():
+    """Test internal construction of collision models for MeshVolumeRegions."""
     r1 = BoxRegion(dimensions=(2, 2, 2)).difference(BoxRegion(dimensions=(1, 1, 3)))
 
     for heading, shouldInt in ((0, False), (math.pi / 4, True), (math.pi / 2, False)):
         o = Orientation.fromEuler(heading, 0, 0)
         r2 = BoxRegion(dimensions=(1.5, 1.5, 0.5), position=(2, 0, 0), rotation=o)
         assert r1.intersects(r2) == shouldInt
 
-        o1 = fcl.CollisionObject(*r1._fclData)
-        o2 = fcl.CollisionObject(*r2._fclData)
-        assert fcl.collide(o1, o2) == shouldInt
+        o1 = coal.CollisionObject(*r1._collisionData)
+        o2 = coal.CollisionObject(*r2._collisionData)
+        req = coal.CollisionRequest()
+        res = coal.CollisionResult()
+        assert bool(coal.collide(o1, o2, req, res)) == shouldInt
 
     bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4)
     r3 = MeshVolumeRegion(r1.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r1)
-    o3 = fcl.CollisionObject(*r3._fclData)
+    o3 = coal.CollisionObject(*r3._collisionData)
     r4pos = r3.position.offsetLocally(bo, (0, 2, 0))
 
     for heading, shouldInt in ((0, False), (math.pi / 4, True), (math.pi / 2, False)):
         o = bo * Orientation.fromEuler(heading, 0, 0)
         r4 = BoxRegion(dimensions=(1.5, 1.5, 0.5), position=r4pos, rotation=o)
         assert r3.intersects(r4) == shouldInt
 
-        o4 = fcl.CollisionObject(*r4._fclData)
-        assert fcl.collide(o3, o4) == shouldInt
+        o4 = coal.CollisionObject(*r4._collisionData)
+        req = coal.CollisionRequest()
+        res = coal.CollisionResult()
+        assert bool(coal.collide(o3, o4, req, res)) == shouldInt
 
 
 def test_mesh_empty_intersection():

tests/syntax/test_requirements.py

@@ -478,6 +478,52 @@ def test_static_intersection_violation_disabled():
     )
 
 
+def test_intersection_colliding_objects_identified():
+    """BlanketCollisionRequirement identifies the colliding pair by object identity.
+
+    This is a unit test for the Coal broadphase pair-identification path:
+    falsifiedByInner must set _collidingObjects to the exact two scenic
+    objects whose geometries overlap, with no pairwise fallback loop.
+    """
+    import types
+
+    import coal
+    import numpy
+
+    from scenic.core.requirements import BlanketCollisionRequirement
+
+    # Minimal fake scenic objects — falsifiedByInner only needs these two attrs.
+    # Use a class so instances are hashable (needed for the sample dict key).
+    class FakeObj:
+        def __init__(self, geom, trans=None):
+            self.allowCollisions = False
+            self.occupiedSpace = types.SimpleNamespace()
+            self.occupiedSpace._collisionData = (
+                geom,
+                trans if trans is not None else coal.Transform3s(),
+            )
+
+    def make_obj(geom, trans=None):
+        return FakeObj(geom, trans)
+
+    # obj_a and obj_b overlap (same position); obj_c is far away.
+    obj_a = make_obj(coal.Box(1.0, 1.0, 1.0))
+    obj_b = make_obj(coal.Box(1.0, 1.0, 1.0))
+    obj_c = make_obj(coal.Box(1.0, 1.0, 1.0))
+    obj_c.occupiedSpace._collisionData = (
+        coal.Box(1.0, 1.0, 1.0),
+        coal.Transform3s(numpy.eye(3), numpy.array([100.0, 0.0, 0.0])),
+    )
+
+    req = BlanketCollisionRequirement([obj_a, obj_b, obj_c], optional=True)
+    sample = {obj_a: obj_a, obj_b: obj_b, obj_c: obj_c}
+
+    assert req.falsifiedByInner(sample) is True
+    assert req._collidingObjects is not None
+    # The colliding pair must be exactly obj_a and obj_b — not obj_c.
+    assert set(req._collidingObjects) == {obj_a, obj_b}
+
+
 # Occlusion visibility requirements