diff --git a/docs/install_notes.rst b/docs/install_notes.rst index 68cc293d9..bc264d79c 100644 --- a/docs/install_notes.rst +++ b/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 `_ repository. -2. Navigate to the repository. -3. Install dependencies using `Homebrew `__ 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 ------- diff --git a/pyproject.toml b/pyproject.toml index ef8945493..ccbad904e 100644 --- a/pyproject.toml +++ b/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", diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 38e876d01..a16eac312 100644 --- a/src/scenic/core/regions.py +++ b/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 diff --git a/src/scenic/core/requirements.py b/src/scenic/core/requirements.py index 53633057e..17fb18973 100644 --- a/src/scenic/core/requirements.py +++ b/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 diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index 2f1f0dc8a..840ee6c82 100644 --- a/tests/core/test_regions.py +++ b/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,8 +481,8 @@ 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)): @@ -490,13 +490,15 @@ def test_mesh_fcl(): 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)): @@ -504,8 +506,10 @@ def test_mesh_fcl(): 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(): diff --git a/tests/syntax/test_requirements.py b/tests/syntax/test_requirements.py index d81eeb672..e5ab9faae 100644 --- a/tests/syntax/test_requirements.py +++ b/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