From 444e4189313d0fed0b7da64db5710bd99e62b683 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Thu, 25 Apr 2024 21:10:50 -0700 Subject: [PATCH 01/22] disable redundant centering of occupiedSpace --- src/scenic/core/object_types.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 0fc4cb6df..ba38cec8a 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1304,6 +1304,7 @@ def occupiedSpace(self): dimensions=(self.width, self.length, self.height), position=self.position, rotation=self.orientation, + centerMesh=False, ) @property From 5f5f1171aa9dd4d6081434b9a6897f1221140442 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Thu, 25 Apr 2024 21:11:31 -0700 Subject: [PATCH 02/22] apply MeshRegion transformations simultaneously --- src/scenic/core/regions.py | 27 +++++++++------------------ src/scenic/core/vectors.py | 3 +++ 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index cadd13103..daf2a6f51 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -842,26 +842,17 @@ def __init__( if centerMesh: self.mesh.vertices -= self.mesh.bounding_box.center_mass - # If dimensions are provided, scale mesh to those dimension + # Apply scaling, rotation, and translation, if any if self.dimensions is not None: - scale = self.mesh.extents / numpy.array(self.dimensions) - - scale_matrix = numpy.eye(4) - scale_matrix[:3, :3] /= scale - - self.mesh.apply_transform(scale_matrix) - - # If rotation is provided, apply rotation + scale = numpy.array(self.dimensions) / self.mesh.extents + else: + scale = None if self.rotation is not None: - rotation_matrix = quaternion_matrix( - (self.rotation.w, self.rotation.x, self.rotation.y, self.rotation.z) - ) - self.mesh.apply_transform(rotation_matrix) - - # If position is provided, translate mesh. - if self.position is not None: - position_matrix = translation_matrix(self.position) - self.mesh.apply_transform(position_matrix) + angles = self.rotation._trimeshEulerAngles() + else: + angles = None + matrix = compose_matrix(scale=scale, angles=angles, translate=self.position) + self.mesh.apply_transform(matrix) self.orientation = orientation diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index b90a315b2..0ec044df8 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -311,6 +311,9 @@ def eulerAngles(self) -> typing.Tuple[float, float, float]: """Global intrinsic Euler angles yaw, pitch, roll.""" return self.r.as_euler("ZXY", degrees=False) + def _trimeshEulerAngles(self): + return self.r.as_euler("xyz", degrees=False) + def getRotation(self): return self.r From ee6e0db8d77585c8726fa4f6351a168a93631c0a Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Fri, 26 Apr 2024 19:28:57 -0700 Subject: [PATCH 03/22] reduce redundant checks on occupiedSpace --- src/scenic/core/object_types.py | 5 ++++- src/scenic/core/regions.py | 9 +++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index ba38cec8a..309a9df94 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1299,12 +1299,15 @@ def _corners2D(self): @cached_property def occupiedSpace(self): """A region representing the space this object occupies""" + shape = self.shape return MeshVolumeRegion( - mesh=self.shape.mesh, + mesh=shape.mesh, dimensions=(self.width, self.length, self.height), position=self.position, rotation=self.orientation, centerMesh=False, + _internal=True, + _isConvex=shape.isConvex, ) @property diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index daf2a6f51..5d7bc2433 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1071,8 +1071,9 @@ class MeshVolumeRegion(MeshRegion): onDirection: The direction to use if an object being placed on this region doesn't specify one. """ - def __init__(self, *args, **kwargs): + def __init__(self, *args, _internal=False, _isConvex=None, **kwargs): super().__init__(*args, **kwargs) + self._isConvex = _isConvex if isLazy(self): return @@ -1084,7 +1085,7 @@ def __init__(self, *args, **kwargs): raise ValueError(f"{name} of MeshVolumeRegion must be positive") # Ensure the mesh is a well defined volume - if not self._mesh.is_volume: + if not _internal and not self._mesh.is_volume: raise ValueError( "A MeshVolumeRegion cannot be defined with a mesh that does not have a well defined volume." " Consider using scenic.core.utils.repairMesh." @@ -1738,6 +1739,10 @@ def inradius(self): else: return region_distance + @cached_property + def isConvex(self): + return self.mesh.is_convex if self._isConvex is None else self._isConvex + @property def dimensionality(self): return 3 From 0ec764d3a53db7ed4dd291c9937fd5025b517b53 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Fri, 26 Apr 2024 19:30:34 -0700 Subject: [PATCH 04/22] improve backtrace for errors inside Point @properties --- src/scenic/core/object_types.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 309a9df94..85edf32cc 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -810,9 +810,7 @@ def __getattr__(self, attr): if hasattr(Vector, attr): return getattr(self.toVector(), attr) else: - raise AttributeError( - f"'{type(self).__name__}' object has no attribute '{attr}'" - ) + self.__getattribute__(attr) ## OrientedPoint From 0a428923d73333a62a540a219aa95ecac7885f19 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Fri, 26 Apr 2024 21:17:26 -0700 Subject: [PATCH 05/22] PolygonalFootprintRegion.containsObject checks convex hull before exact bounding polygon --- src/scenic/core/regions.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 5d7bc2433..e24c4792d 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1004,13 +1004,18 @@ def AABB(self): tuple(self.mesh.bounds[:, 2]), ) + @cached_property + def _boundingPolygonHull(self): + assert not isLazy(self) + return shapely.multipoints(self.mesh.vertices).convex_hull + @cached_property def _boundingPolygon(self): assert not isLazy(self) # Relatively fast case for convex regions if self.isConvex: - return shapely.geometry.MultiPoint(self.mesh.vertices).convex_hull + return self._boundingPolygonHull # Generic case for arbitrary shapes if self.mesh.is_watertight: @@ -2420,7 +2425,18 @@ def containsObject(self, obj): Args: obj: An object to be checked for containment. """ - # Check containment using the bounding polygon of the object. + # Fast path for convex objects, whose bounding polygons are relatively + # easy to compute. + if obj._isConvex: + return self.polygons.contains(obj._boundingPolygon) + + # Quick check using the projected convex hull of the object, which + # overapproximates the actual bounding polygon. + hullPoly = obj.occupiedSpace._boundingPolygonHull + if self.polygons.contains(hullPoly): + return True + + # Need to compute exact bounding polygon. return self.polygons.contains(obj._boundingPolygon) def containsRegionInner(self, reg, tolerance): From 3186598c50ee9094f9728eb6cb043e7b7791e70f Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sat, 27 Apr 2024 08:52:47 -0700 Subject: [PATCH 06/22] speed up bounding box intersection check --- src/scenic/core/regions.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index e24c4792d..529b516a9 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1123,11 +1123,13 @@ def intersects(self, other, triedReversed=False): # Check if bounding boxes intersect. If not, volumes cannot intersect. # For bounding boxes to intersect there must be overlap of the bounds # in all 3 dimensions. - range_overlaps = [ - (self.mesh.bounds[0, dim] <= other.mesh.bounds[1, dim]) - and (other.mesh.bounds[0, dim] <= self.mesh.bounds[1, dim]) + bounds = self._mesh.bounds + obounds = other._mesh.bounds + range_overlaps = ( + (bounds[0, dim] <= obounds[1, dim]) + and (obounds[0, dim] <= bounds[1, dim]) for dim in range(3) - ] + ) bb_overlap = all(range_overlaps) if not bb_overlap: From 13baa008504fe99cdc68a5f722ed0f8185711b50 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sat, 27 Apr 2024 12:37:13 -0700 Subject: [PATCH 07/22] built-in mutators directly modify objects --- src/scenic/core/object_types.py | 23 +++++++++++------------ src/scenic/simulators/utils/colors.py | 4 ++-- tests/syntax/test_basic.py | 8 ++++++-- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 85edf32cc..1604c36ca 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -613,15 +613,15 @@ class Mutator: """ def appliedTo(self, obj): - """Return a mutated copy of the given object. Implemented by subclasses. + """Return a mutated version of the given object. Implemented by subclasses. The mutator may inspect the ``mutationScale`` attribute of the given object to scale its effect according to the scale given in ``mutate O by S``. Returns: - A pair consisting of the mutated copy of the object (which is most easily - created using `_copyWith`) together with a Boolean indicating whether the - mutator inherited from the superclass (if any) should also be applied. + A pair consisting of the mutated version of the object together with a + Boolean indicating whether the mutator inherited from the superclass + (if any) should also be applied. """ raise NotImplementedError @@ -642,8 +642,8 @@ def appliedTo(self, obj): random.gauss(0, self.stddevs[1] * obj.mutationScale), random.gauss(0, self.stddevs[2] * obj.mutationScale), ) - pos = obj.position + noise - return (obj._copyWith(position=pos), True) # allow further mutation + obj.position += noise + return (obj, True) # allow further mutation def __eq__(self, other): if type(other) is not type(self): @@ -665,13 +665,11 @@ def __init__(self, stddevs): self.stddevs = tuple(stddevs) def appliedTo(self, obj): - yaw = obj.yaw + random.gauss(0, self.stddevs[0] * obj.mutationScale) - pitch = obj.pitch + random.gauss(0, self.stddevs[1] * obj.mutationScale) - roll = obj.roll + random.gauss(0, self.stddevs[2] * obj.mutationScale) + obj.yaw += random.gauss(0, self.stddevs[0] * obj.mutationScale) + obj.pitch += random.gauss(0, self.stddevs[1] * obj.mutationScale) + obj.roll += random.gauss(0, self.stddevs[2] * obj.mutationScale) - new_obj = obj._copyWith(yaw=yaw, pitch=pitch, roll=roll) - - return (new_obj, True) # allow further mutation + return (obj, True) # allow further mutation def __eq__(self, other): if type(other) is not type(self): @@ -803,6 +801,7 @@ def sampleGiven(self, value): sample, proceed = mutator.appliedTo(sample) if not proceed: break + sample._recomputeDynamicFinals() return sample # Points automatically convert to Vectors when needed diff --git a/src/scenic/simulators/utils/colors.py b/src/scenic/simulators/utils/colors.py index 931788b21..7f3a0bed9 100644 --- a/src/scenic/simulators/utils/colors.py +++ b/src/scenic/simulators/utils/colors.py @@ -119,7 +119,7 @@ def appliedTo(self, obj): hueNoise = random.gauss(0, stddev) satNoise = random.gauss(0, stddev) lightNoise = random.gauss(0, stddev) - color = NoisyColorDistribution.addNoiseTo( + obj.color = NoisyColorDistribution.addNoiseTo( obj.color, hueNoise, lightNoise, satNoise ) - return (obj._copyWith(color=color), True) # allow further mutation + return (obj, True) # allow further mutation diff --git a/tests/syntax/test_basic.py b/tests/syntax/test_basic.py index 3fff22c63..0a2a75cdc 100644 --- a/tests/syntax/test_basic.py +++ b/tests/syntax/test_basic.py @@ -111,7 +111,10 @@ def test_param_write(): def test_mutate(): scenario = compileScenic( """ - ego = new Object at 3@1, facing 0 + class Thing: + foo: self.heading + + ego = new Thing at 3@1, facing 0 mutate """ ) @@ -119,6 +122,7 @@ def test_mutate(): assert ego1.position.x != pytest.approx(3) assert ego1.position.y != pytest.approx(1) assert ego1.heading != pytest.approx(0) + assert ego1.foo == 0 def test_mutate_object(): @@ -158,7 +162,7 @@ def test_mutate_nonobject(): """ ego = new Object mutate sin - """ + """ ) From 2555912abda45b52f049040233079e802f37a695 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 28 Apr 2024 20:41:43 -0700 Subject: [PATCH 08/22] [WIP] precompute geometric data when shape/dimensions are fixed --- src/scenic/core/object_types.py | 55 ++++++++++- src/scenic/core/regions.py | 153 ++++++++++++++++++------------- tests/core/test_serialization.py | 1 + 3 files changed, 143 insertions(+), 66 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 1604c36ca..217075666 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -16,6 +16,8 @@ from abc import ABC, abstractmethod import collections +import functools +import inspect import math import random import typing @@ -77,7 +79,7 @@ toVector, underlyingType, ) -from scenic.core.utils import DefaultIdentityDict, cached_method, cached_property +from scenic.core.utils import DefaultIdentityDict, cached, cached_method, cached_property from scenic.core.vectors import ( Orientation, Vector, @@ -214,6 +216,7 @@ def __init__(self, properties, constProps=frozenset(), _internal=False): self.properties = tuple(sorted(properties.keys())) self._propertiesSet = set(self.properties) self._constProps = constProps + self._sampleParent = None @classmethod def _withProperties(cls, properties, constProps=None): @@ -544,7 +547,9 @@ def sampleGiven(self, value): if not needsSampling(self): return self props = {prop: value[getattr(self, prop)] for prop in self.properties} - return type(self)(props, constProps=self._constProps, _internal=True) + obj = type(self)(props, constProps=self._constProps, _internal=True) + obj._sampleParent = self + return obj def _allProperties(self): return {prop: getattr(self, prop) for prop in self.properties} @@ -599,6 +604,35 @@ def __repr__(self): return f"{type(self).__name__}({allProps})" +def precomputed_property(func): + """A @property which can be precomputed if its dependencies are not random. + + Converts a function inside a subclass of `Constructible` into a method; the + function's arguments must correspond to the properties of the object needed + to compute this property. If any of those dependencies have random values, + this property will evaluate to `None`; otherwise it will be computed once + the first time it is needed and then reused across samples. + """ + deps = tuple(inspect.signature(func).parameters) + + @cached + @functools.wraps(func) + def method(self): + args = [getattr(self, prop) for prop in deps] + if any(needsSampling(arg) for arg in args): + return None + return func(*args) + + @functools.wraps(func) + def wrapper(self): + parent = self._sampleParent or self + return method(parent) + + wrapper._scenic_cache_clearer = method._scenic_cache_clearer + + return property(wrapper) + + ## Mutators @@ -1297,6 +1331,12 @@ def _corners2D(self): def occupiedSpace(self): """A region representing the space this object occupies""" shape = self.shape + ss = self._scaledShape + if ss is not None: + candData = (ss._candidatePoint, ss._candidateRadii) + volume = ss._mesh.volume + else: + candData = volume = None return MeshVolumeRegion( mesh=shape.mesh, dimensions=(self.width, self.length, self.height), @@ -1305,6 +1345,17 @@ def occupiedSpace(self): centerMesh=False, _internal=True, _isConvex=shape.isConvex, + _candidatePointData=candData, + _volume=volume, + ) + + @precomputed_property + def _scaledShape(shape, width, length, height): + return MeshVolumeRegion( + mesh=shape.mesh, + dimensions=(width, length, height), + centerMesh=False, + _internal=True, ) @property diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 529b516a9..f8f0ece00 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -851,8 +851,11 @@ def __init__( angles = self.rotation._trimeshEulerAngles() else: angles = None - matrix = compose_matrix(scale=scale, angles=angles, translate=self.position) - self.mesh.apply_transform(matrix) + self._transform = compose_matrix( + scale=scale, angles=angles, translate=self.position + ) + self._rigidTransform = compose_matrix(angles=angles, translate=self.position) + self.mesh.apply_transform(self._transform) self.orientation = orientation @@ -1076,9 +1079,18 @@ class MeshVolumeRegion(MeshRegion): onDirection: The direction to use if an object being placed on this region doesn't specify one. """ - def __init__(self, *args, _internal=False, _isConvex=None, **kwargs): + def __init__( + self, + *args, + _internal=False, + _isConvex=None, + _candidatePointData=None, + _volume=None, + **kwargs, + ): super().__init__(*args, **kwargs) self._isConvex = _isConvex + self._candidatePointData = _candidatePointData if isLazy(self): return @@ -1098,7 +1110,8 @@ def __init__(self, *args, _internal=False, _isConvex=None, **kwargs): # Compute how many samples are necessary to achieve 99% probability # of success when rejection sampling volume. - p_volume = self._mesh.volume / self._mesh.bounding_box.volume + volume = _volume or self._mesh.volume + p_volume = volume / self._mesh.bounding_box.volume if p_volume > 0.99: self.num_samples = 1 @@ -1142,57 +1155,21 @@ def intersects(self, other, triedReversed=False): # If the candidate points are farther apart than the sum of the circumradius # values, they can't intersect. - # Get a candidate point from each mesh. If the center of the object is in the mesh use that. - # Otherwise try to sample a point as a candidate, skipping this pass if the sample fails. - if self.containsPoint(Vector(*self.mesh.bounding_box.center_mass)): - s_candidate_point = Vector(*self.mesh.bounding_box.center_mass) - elif ( - len(samples := trimesh.sample.volume_mesh(self.mesh, self.num_samples)) - > 0 - ): - s_candidate_point = Vector(*samples[0]) - else: - s_candidate_point = None - - if other.containsPoint(Vector(*other.mesh.bounding_box.center_mass)): - o_candidate_point = Vector(*other.mesh.bounding_box.center_mass) - elif ( - len(samples := trimesh.sample.volume_mesh(other.mesh, other.num_samples)) - > 0 - ): - o_candidate_point = Vector(*samples[0]) - else: - o_candidate_point = None - - if s_candidate_point is not None and o_candidate_point is not None: - # Compute the inradius of each object from its candidate point. - s_inradius = abs( - trimesh.proximity.ProximityQuery(self.mesh).signed_distance( - [s_candidate_point] - )[0] - ) - o_inradius = abs( - trimesh.proximity.ProximityQuery(other.mesh).signed_distance( - [o_candidate_point] - )[0] - ) + # Get a candidate point from each mesh and the corresponding + # inradius/circumradius around that point. + s_candidate_point = self._candidatePoint + s_inradius, s_circumradius = self._candidateRadii + o_candidate_point = other._candidatePoint + o_inradius, o_circumradius = other._candidateRadii - # Compute the circumradius of each object from its candidate point. - s_circumradius = numpy.max( - numpy.linalg.norm(self.mesh.vertices - s_candidate_point, axis=1) - ) - o_circumradius = numpy.max( - numpy.linalg.norm(other.mesh.vertices - o_candidate_point, axis=1) - ) + # Get the distance between the two points and check for mandatory or impossible collision. + point_distance = numpy.linalg.norm(s_candidate_point - o_candidate_point) - # Get the distance between the two points and check for mandatory or impossible collision. - point_distance = s_candidate_point.distanceTo(o_candidate_point) - - if point_distance < s_inradius + o_inradius: - return True + if point_distance < s_inradius + o_inradius: + return True - if point_distance > s_circumradius + o_circumradius: - return False + if point_distance > s_circumradius + o_circumradius: + return False # PASS 3 # Use Trimesh's collision manager to check for intersection. @@ -1200,15 +1177,15 @@ def intersects(self, other, triedReversed=False): # Cheaper than computing volumes immediately. collision_manager = trimesh.collision.CollisionManager() - collision_manager.add_object("SelfRegion", self.mesh) - collision_manager.add_object("OtherRegion", other.mesh) + collision_manager.add_object("SelfRegion", self._mesh) + collision_manager.add_object("OtherRegion", other._mesh) surface_collision = collision_manager.in_collision_internal() if surface_collision: return True - if self.mesh.is_convex and other.mesh.is_convex: + if self.isConvex and other.isConvex: # For convex shapes, the manager detects containment as well as # surface intersections, so we can just return the result return surface_collision @@ -1217,15 +1194,10 @@ def intersects(self, other, triedReversed=False): # If we have 2 candidate points and both regions have only one body, # we can just check if either region contains the candidate point of the # other. (This is because we previously ruled out surface intersections) - if ( - s_candidate_point is not None - and o_candidate_point is not None - and self.mesh.body_count == 1 - and other.mesh.body_count == 1 - ): - return self.containsPoint(o_candidate_point) or other.containsPoint( - s_candidate_point - ) + if self.mesh.body_count == 1 and other.mesh.body_count == 1: + return self._containsPointExact( + o_candidate_point + ) or other._containsPointExact(s_candidate_point) # PASS 5 # Compute intersection and check if it's empty. Expensive but guaranteed @@ -1292,6 +1264,9 @@ def containsPoint(self, point): """Check if this region's volume contains a point.""" return self.distanceTo(point) <= self.tolerance + def _containsPointExact(self, point): + return self._mesh.contains([point])[0] + @distributionFunction def containsObject(self, obj): """Check if this region's volume contains an :obj:`~scenic.core.object_types.Object`.""" @@ -1837,6 +1812,56 @@ def getVolumeRegion(self): """Returns this object, as it is already a MeshVolumeRegion""" return self + @cached_property + def _candidatePoint(self): + # Use precomputed point if available (transformed appropriately) + if self._candidatePointData: + raw = self._candidatePointData[0] + homog = numpy.append(raw, [1]) + return numpy.dot(self._rigidTransform, homog)[:3] + + # Use center of mass if it's contained + mesh = self._mesh + com = mesh.bounding_box.center_mass + if mesh.contains([com])[0]: + return com + + # Try sampling a point inside the volume + samples = trimesh.sample.volume_mesh(mesh, self.num_samples) + if samples.size > 0: + # Found at least one interior point; choose the closest to the CoM + distances = numpy.linalg.norm(samples - com) + return samples[numpy.argmin(distances)] + + # If all else fails, take a point from the surface and move inward + surfacePt, index = list(zip(*mesh.sample(1, return_index=True)))[0] + inward = -mesh.face_normals[index] + startPt = surfacePt + 1e-6 * inward + hits, _, _ = mesh.ray.intersects_location( + ray_origins=[startPt], + ray_directions=[inward], + multiple_hits=False, + ) + if hits.size > 0: + endPt = hits[0] + midPt = (surfacePt + endPt) / 2.0 + if mesh.contains([midPt])[0]: + return midPt + return surfacePt + + @cached_property + def _candidateRadii(self): + # Use precomputed radii if available + if self._candidatePointData: + return self._candidatePointData[1] + + # Compute inradius and circumradius w.r.t. the candidate point + point = self._candidatePoint + pq = trimesh.proximity.ProximityQuery(self._mesh) + inradius = abs(pq.signed_distance([point])[0]) + circumradius = numpy.max(numpy.linalg.norm(self._mesh.vertices - point, axis=1)) + return inradius, circumradius + class MeshSurfaceRegion(MeshRegion): """A region representing the surface of a mesh. diff --git a/tests/core/test_serialization.py b/tests/core/test_serialization.py index af0b6705e..465890b7d 100644 --- a/tests/core/test_serialization.py +++ b/tests/core/test_serialization.py @@ -54,6 +54,7 @@ def assertSceneEquivalence(scene1, scene2, ignoreDynamics=False, ignoreConstProp if ignoreDynamics: del scene1.dynamicScenario, scene2.dynamicScenario for obj in scene1.objects + scene2.objects: + del obj._sampleParent if ignoreConstProps: del obj._constProps if ignoreDynamics: From 7b4f06cdbdd2c34ab680116cc962ae6792379bfb Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 15:29:52 -0800 Subject: [PATCH 09/22] precompute geometric data when shape/dimensions are fixed --- src/scenic/core/object_types.py | 9 +---- src/scenic/core/regions.py | 72 +++++++++++++++++++++------------ src/scenic/core/requirements.py | 39 ++++++++++++++---- tests/core/test_regions.py | 57 +++++++++++++++++++++++++- 4 files changed, 135 insertions(+), 42 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 9d6deda6f..f42cb9ca0 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1332,12 +1332,6 @@ def _corners2D(self): def occupiedSpace(self): """A region representing the space this object occupies""" shape = self.shape - ss = self._scaledShape - if ss is not None: - candData = (ss._candidatePoint, ss._candidateRadii) - volume = ss._mesh.volume - else: - candData = volume = None return MeshVolumeRegion( mesh=shape.mesh, dimensions=(self.width, self.length, self.height), @@ -1346,8 +1340,7 @@ def occupiedSpace(self): centerMesh=False, _internal=True, _isConvex=shape.isConvex, - _candidatePointData=candData, - _volume=volume, + _scaledShape=self._scaledShape, ) @precomputed_property diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 0e3aac843..0df264285 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -13,6 +13,7 @@ import random import warnings +import fcl import numpy import scipy import shapely @@ -1083,13 +1084,12 @@ def __init__( *args, _internal=False, _isConvex=None, - _candidatePointData=None, - _volume=None, + _scaledShape=None, **kwargs, ): super().__init__(*args, **kwargs) self._isConvex = _isConvex - self._candidatePointData = _candidatePointData + self._scaledShape = _scaledShape if isLazy(self): return @@ -1109,13 +1109,13 @@ def __init__( # Compute how many samples are necessary to achieve 99% probability # of success when rejection sampling volume. - volume = _volume or self._mesh.volume + volume = _scaledShape._mesh.volume if _scaledShape else self._mesh.volume p_volume = volume / self._mesh.bounding_box.volume if p_volume > 0.99: self.num_samples = 1 else: - self.num_samples = min(1e6, max(1, math.ceil(math.log(0.01, 1 - p_volume)))) + self.num_samples = math.ceil(min(1e6, max(1, math.log(0.01, 1 - p_volume)))) # Always try to take at least 8 samples to avoid surface point total rejections self.num_samples = max(self.num_samples, 8) @@ -1156,13 +1156,13 @@ def intersects(self, other, triedReversed=False): # Get a candidate point from each mesh and the corresponding # inradius/circumradius around that point. - s_candidate_point = self._candidatePoint + s_point = self._candidatePoint s_inradius, s_circumradius = self._candidateRadii - o_candidate_point = other._candidatePoint + o_point = other._candidatePoint o_inradius, o_circumradius = other._candidateRadii # Get the distance between the two points and check for mandatory or impossible collision. - point_distance = numpy.linalg.norm(s_candidate_point - o_candidate_point) + point_distance = numpy.linalg.norm(s_point - o_point) if point_distance < s_inradius + o_inradius: return True @@ -1171,21 +1171,19 @@ def intersects(self, other, triedReversed=False): return False # PASS 3 - # Use Trimesh's collision manager to check for intersection. + # Use FCL to check for intersection between the surfaces. # If the surfaces collide, that implies a collision of the volumes. # Cheaper than computing volumes immediately. - collision_manager = trimesh.collision.CollisionManager() - collision_manager.add_object("SelfRegion", self._mesh) - collision_manager.add_object("OtherRegion", other._mesh) - - surface_collision = collision_manager.in_collision_internal() + selfObj = fcl.CollisionObject(*self._fclData) + otherObj = fcl.CollisionObject(*other._fclData) + surface_collision = fcl.collide(selfObj, otherObj) if surface_collision: return True if self.isConvex and other.isConvex: - # For convex shapes, the manager detects containment as well as + # For convex shapes, FCL detects containment as well as # surface intersections, so we can just return the result return surface_collision @@ -1194,9 +1192,9 @@ def intersects(self, other, triedReversed=False): # we can just check if either region contains the candidate point of the # other. (This is because we previously ruled out surface intersections) if self.mesh.body_count == 1 and other.mesh.body_count == 1: - return self._containsPointExact( - o_candidate_point - ) or other._containsPointExact(s_candidate_point) + return self._containsPointExact(o_point) or other._containsPointExact( + s_point + ) # PASS 5 # Compute intersection and check if it's empty. Expensive but guaranteed @@ -1814,8 +1812,8 @@ def getVolumeRegion(self): @cached_property def _candidatePoint(self): # Use precomputed point if available (transformed appropriately) - if self._candidatePointData: - raw = self._candidatePointData[0] + if self._scaledShape: + raw = self._scaledShape._candidatePoint homog = numpy.append(raw, [1]) return numpy.dot(self._rigidTransform, homog)[:3] @@ -1828,9 +1826,7 @@ def _candidatePoint(self): # Try sampling a point inside the volume samples = trimesh.sample.volume_mesh(mesh, self.num_samples) if samples.size > 0: - # Found at least one interior point; choose the closest to the CoM - distances = numpy.linalg.norm(samples - com) - return samples[numpy.argmin(distances)] + return samples[0] # If all else fails, take a point from the surface and move inward surfacePt, index = list(zip(*mesh.sample(1, return_index=True)))[0] @@ -1851,8 +1847,8 @@ def _candidatePoint(self): @cached_property def _candidateRadii(self): # Use precomputed radii if available - if self._candidatePointData: - return self._candidatePointData[1] + if self._scaledShape: + return self._scaledShape._candidateRadii # Compute inradius and circumradius w.r.t. the candidate point point = self._candidatePoint @@ -1861,6 +1857,32 @@ def _candidateRadii(self): circumradius = numpy.max(numpy.linalg.norm(self._mesh.vertices - point, axis=1)) return inradius, circumradius + @cached_property + def _fclData(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)) + 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()) + else: + geom = fcl.BVHModel() + geom.beginModel(num_tris_=len(mesh.faces), num_vertices_=len(mesh.vertices)) + geom.addSubModel(mesh.vertices, mesh.faces) + geom.endModel() + trans = fcl.Transform() + return geom, trans + + def __getstate__(self): + state = self.__dict__.copy() + state.pop("_cached__fclData", None) # remove non-picklable FCL objects + return state + class MeshSurfaceRegion(MeshRegion): """A region representing the surface of a mesh. diff --git a/src/scenic/core/requirements.py b/src/scenic/core/requirements.py index ab6de91da..287ec98c2 100644 --- a/src/scenic/core/requirements.py +++ b/src/scenic/core/requirements.py @@ -6,6 +6,8 @@ import inspect import itertools +import fcl +import numpy import rv_ltl import trimesh @@ -319,6 +321,8 @@ def violationMsg(self): class IntersectionRequirement(SamplingRequirement): + """Requirement that a pair of objects do not intersect.""" + def __init__(self, objA, objB, optional=False): super().__init__(optional=optional) self.objA = objA @@ -337,6 +341,16 @@ def violationMsg(self): class BlanketCollisionRequirement(SamplingRequirement): + """Requirement that the surfaces of a given set of objects do not intersect. + + We can check for such intersections more quickly than full objects using FCL, + but since FCL checks for surface intersections rather than volume intersections + (in general), this requirement being satisfied does not imply that the objects + do not intersect (one might still be contained in another). Therefore, this + requirement is intended to quickly check for intersections in the common case + rather than completely determine whether any objects collide. + """ + def __init__(self, objects, optional=True): super().__init__(optional=optional) self.objects = objects @@ -344,23 +358,32 @@ def __init__(self, objects, optional=True): def falsifiedByInner(self, sample): objects = tuple(sample[obj] for obj in self.objects) - cm = trimesh.collision.CollisionManager() + manager = fcl.DynamicAABBTreeCollisionManager() + objForGeom = {} for i, obj in enumerate(objects): - if not obj.allowCollisions: - cm.add_object(str(i), obj.occupiedSpace.mesh) - collision, names = cm.in_collision_internal(return_names=True) + if obj.allowCollisions: + continue + geom, trans = obj.occupiedSpace._fclData + collisionObject = fcl.CollisionObject(geom, trans) + objForGeom[geom] = obj + manager.registerObject(collisionObject) + + manager.setup() + cdata = fcl.CollisionData() + manager.collide(cdata, fcl.defaultCollisionCallback) + collision = cdata.result.is_collision if collision: - self._collidingObjects = tuple(sorted(names)) + contact = cdata.result.contacts[0] + self._collidingObjects = (objForGeom[contact.o1], objForGeom[contact.o2]) return collision @property def violationMsg(self): assert self._collidingObjects is not None - objA_index, objB_index = map(int, self._collidingObjects[0]) - objA, objB = self.objects[objA_index], self.objects[objB_index] - return f"Intersection violation: {objA} intersects {objB}" + objA, objB = self._collidingObjects + return f"Blanket Intersection violation: {objA} intersects {objB}" class ContainmentRequirement(SamplingRequirement): diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index b7293ab3c..e6029fb35 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -1,6 +1,7 @@ import math from pathlib import Path +import fcl import pytest import shapely.geometry import trimesh.voxel @@ -8,7 +9,7 @@ from scenic.core.distributions import RandomControlFlowError, Range from scenic.core.object_types import Object, OrientedPoint from scenic.core.regions import * -from scenic.core.vectors import VectorField +from scenic.core.vectors import Orientation, VectorField from tests.utils import deprecationTest, sampleSceneFrom @@ -338,6 +339,60 @@ def test_mesh_intersects(): assert not r1.getSurfaceRegion().intersects(r2.getSurfaceRegion()) +def test_mesh_candidatePoint(): + regions = [ + BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)), + BoxRegion().difference(BoxRegion(dimensions=(0.1, 0.1, 0.1))), + ] + d = 1e6 + r = BoxRegion(dimensions=(d, d, d)).difference( + BoxRegion(dimensions=(d - 1, d - 1, d - 1)) + ) + r.num_samples = 8 # ensure sampling won't yield a good point + regions.append(r) + + bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4) + r2 = MeshVolumeRegion(r.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r) + regions.append(r2) + + for reg in regions: + cp = reg._candidatePoint + # N.B. _containsPointExact can fail with embreex installed! + assert reg.containsPoint(cp) + inr, circumr = reg._candidateRadii + d = 1.99 * inr + assert reg.containsRegion(SpheroidRegion(dimensions=(d, d, d), position=cp)) + d = 2.01 * circumr + assert SpheroidRegion(dimensions=(d, d, d), position=cp).containsRegion(reg) + + +def test_mesh_fcl(): + """Test internal construction of FCL 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 + + 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) + 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 + + def test_mesh_empty_intersection(): r1 = BoxRegion(position=(0, 0, 0)) r2 = BoxRegion(position=(10, 10, 10)) From 5a9105e56494acd667f685c11e15aa2724c60ae4 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 15:30:55 -0800 Subject: [PATCH 10/22] improve requirement sorting heuristic --- src/scenic/core/sample_checking.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/scenic/core/sample_checking.py b/src/scenic/core/sample_checking.py index 3a96826df..040d9a23a 100644 --- a/src/scenic/core/sample_checking.py +++ b/src/scenic/core/sample_checking.py @@ -106,7 +106,7 @@ def sortedRequirements(self): """Return the list of requirements in sorted order""" # Extract and sort active requirements reqs = [req for req in self.requirements if req.active] - reqs.sort(key=self.getWeightedAcceptanceProb) + reqs.sort(key=self.getRequirementCost) # Remove any optional requirements at the end of the list, since they're useless while reqs and reqs[-1].optional: @@ -131,6 +131,13 @@ def updateMetrics(self, req, new_metrics): sum_time += new_time - old_time self.bufferSums[req] = (sum_acc, sum_time) - def getWeightedAcceptanceProb(self, req): + def getRequirementCost(self, req): + # Expected cost of a requirement is average runtime divided by rejection probability; + # if estimated rejection probability is zero, break ties using runtime. sum_acc, sum_time = self.bufferSums[req] - return (sum_acc / self.bufferSize) * (sum_time / self.bufferSize) + runtime = sum_time / self.bufferSize + rej_prob = 1 - (sum_acc / self.bufferSize) + if rej_prob > 0: + return (runtime / rej_prob, 0) + else: + return (float("inf"), runtime) From 0fb3f2ad63b9d59aaafde58a63edb7354869ad7d Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 15:31:49 -0800 Subject: [PATCH 11/22] fix collision detection benchmarking --- src/scenic/core/scenarios.py | 5 +++-- .../collisions/benchmark_collisions.py | 3 +++ .../collisions/city_intersection.scenic | 2 +- .../collisions/narrowGoalNew.scenic | 8 +++---- tools/benchmarking/collisions/vacuum.scenic | 22 +++++++++---------- 5 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index 133911013..fa93d454b 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -495,8 +495,9 @@ def generateDefaultRequirements(self): requirements = [] ## Optional Requirements ## - # Any collision indicates an intersection - requirements.append(BlanketCollisionRequirement(self.objects)) + # Any collision between object surfaces indicates an intersection + if INITIAL_COLLISION_CHECK: + requirements.append(BlanketCollisionRequirement(self.objects)) ## Mandatory Requirements ## # Pairwise object intersection diff --git a/tools/benchmarking/collisions/benchmark_collisions.py b/tools/benchmarking/collisions/benchmark_collisions.py index bfb36be7f..b68fcc349 100644 --- a/tools/benchmarking/collisions/benchmark_collisions.py +++ b/tools/benchmarking/collisions/benchmark_collisions.py @@ -71,6 +71,9 @@ def run_benchmark(path, params): results[(str((benchmark, benchmark_params)), param)] = results_val + # for setup, subresults in results.items(): + # print(f"{setup}: {subresults[0][1]:.2f} s") + # Plot times import matplotlib.pyplot as plt diff --git a/tools/benchmarking/collisions/city_intersection.scenic b/tools/benchmarking/collisions/city_intersection.scenic index e24170e04..268648e4c 100644 --- a/tools/benchmarking/collisions/city_intersection.scenic +++ b/tools/benchmarking/collisions/city_intersection.scenic @@ -15,7 +15,7 @@ from pathlib import Path class EgoCar(WebotsObject): webotsName: "EGO" - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "bmwx5_hull.obj.bz2", initial_rotation=(90 deg, 0, 0)) + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "bmwx5_hull.obj.bz2", initial_rotation=(90 deg, 0, 0)) positionOffset: Vector(-1.43580750, 0, -0.557354985).rotatedBy(Orientation.fromEuler(*self.orientationOffset)) cameraOffset: Vector(-1.43580750, 0, -0.557354985) + Vector(1.72, 0, 1.4) orientationOffset: (90 deg, 0, 0) diff --git a/tools/benchmarking/collisions/narrowGoalNew.scenic b/tools/benchmarking/collisions/narrowGoalNew.scenic index 2d5302098..535a6d443 100644 --- a/tools/benchmarking/collisions/narrowGoalNew.scenic +++ b/tools/benchmarking/collisions/narrowGoalNew.scenic @@ -12,7 +12,7 @@ workspace = Workspace(RectangularRegion(0 @ 0, 0, width, length)) class MarsGround(Ground): width: width length: length - color: (220, 114, 9) + #color: (220, 114, 9) gridSize: 20 class MarsHill(Hill): @@ -28,7 +28,7 @@ class Goal(WebotsObject): width: 0.1 length: 0.1 webotsType: 'GOAL' - color: (9 ,163, 220) + #color: (9 ,163, 220) class Rover(WebotsObject): """Mars rover.""" @@ -45,14 +45,14 @@ class Debris(WebotsObject): class BigRock(Debris): """Large rock.""" - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "webots_rock_large.obj.bz2") + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "webots_rock_large.obj.bz2") yaw: Range(0, 360 deg) webotsType: 'ROCK_BIG' positionOffset: Vector(0,0, -self.height/2) class Rock(Debris): """Small rock.""" - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "webots_rock_small.obj.bz2") + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "webots_rock_small.obj.bz2") yaw: Range(0, 360 deg) webotsType: 'ROCK_SMALL' positionOffset: Vector(0,0, -self.height/2) diff --git a/tools/benchmarking/collisions/vacuum.scenic b/tools/benchmarking/collisions/vacuum.scenic index e1701c948..e9ef1f157 100644 --- a/tools/benchmarking/collisions/vacuum.scenic +++ b/tools/benchmarking/collisions/vacuum.scenic @@ -30,54 +30,54 @@ class Floor(Object): length: 5 height: 0.01 position: (0,0,-0.005) - color: [200, 200, 200] + #color: [200, 200, 200] class Wall(WebotsObject): webotsAdhoc: {'physics': False} width: 5 length: 0.04 height: 0.5 - color: [160, 160, 160] + #color: [160, 160, 160] class DiningTable(WebotsObject): webotsAdhoc: {'physics': True} - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "dining_table.obj.bz2") + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "dining_table.obj.bz2") width: Range(0.7, 1.5) length: Range(0.7, 1.5) height: 0.75 density: 670 # Density of solid birch - color: [103, 71, 54] + #color: [103, 71, 54] class DiningChair(WebotsObject): webotsAdhoc: {'physics': True} - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "dining_chair.obj.bz2", initial_rotation=(180 deg, 0, 0)) + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "dining_chair.obj.bz2", initial_rotation=(180 deg, 0, 0)) width: 0.4 length: 0.4 height: 1 density: 670 # Density of solid birch positionStdDev: (0.05, 0.05 ,0) orientationStdDev: (10 deg, 0, 0) - color: [103, 71, 54] + #color: [103, 71, 54] class Couch(WebotsObject): webotsAdhoc: {'physics': False} - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "couch.obj.bz2", initial_rotation=(-90 deg, 0, 0)) + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "couch.obj.bz2", initial_rotation=(-90 deg, 0, 0)) width: 2 length: 0.75 height: 0.75 positionStdDev: (0.05, 0.5 ,0) orientationStdDev: (5 deg, 0, 0) - color: [51, 51, 255] + #color: [51, 51, 255] class CoffeeTable(WebotsObject): webotsAdhoc: {'physics': False} - shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "tools" / "meshes" / "coffee_table.obj.bz2") + shape: MeshShape.fromFile(Path(localPath(".")).parent.parent.parent / "assets" / "meshes" / "coffee_table.obj.bz2") width: 1.5 length: 0.5 height: 0.4 positionStdDev: (0.05, 0.05 ,0) orientationStdDev: (5 deg, 0, 0) - color: [103, 71, 54] + #color: [103, 71, 54] class Toy(WebotsObject): webotsAdhoc: {'physics': True} @@ -86,7 +86,7 @@ class Toy(WebotsObject): length: 0.1 height: 0.1 density: 100 - color: [255, 128, 0] + #color: [255, 128, 0] class BlockToy(Toy): shape: BoxShape() From 8631c4f291640a94d21f618dc07dc70c7b90207c Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 15:47:44 -0800 Subject: [PATCH 12/22] precompute body count of MeshVolumeRegion --- src/scenic/core/regions.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 0df264285..ce4140484 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1191,7 +1191,7 @@ def intersects(self, other, triedReversed=False): # If we have 2 candidate points and both regions have only one body, # we can just check if either region contains the candidate point of the # other. (This is because we previously ruled out surface intersections) - if self.mesh.body_count == 1 and other.mesh.body_count == 1: + if self._bodyCount == 1 and other._bodyCount == 1: return self._containsPointExact(o_point) or other._containsPointExact( s_point ) @@ -1857,6 +1857,14 @@ def _candidateRadii(self): circumradius = numpy.max(numpy.linalg.norm(self._mesh.vertices - point, axis=1)) return inradius, circumradius + @cached_property + def _bodyCount(self): + # Use precomputed geometry if available + if self._scaledShape: + return self._scaledShape._bodyCount + + return self.mesh.body_count + @cached_property def _fclData(self): # Use precomputed geometry if available From 1f2d659381e6b56ca34320be9c17524ae78d3ad2 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 15:53:27 -0800 Subject: [PATCH 13/22] speed up copying trimeshes when creating MeshRegions --- pyproject.toml | 2 +- src/scenic/core/regions.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 960bc4724..52069d79f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -47,7 +47,7 @@ dependencies = [ "scikit-image ~= 0.21", "scipy ~= 1.7", "shapely ~= 2.0", - "trimesh >=4.0.9, <5", + "trimesh >=4.4.8, <5", ] [project.optional-dependencies] diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index ce4140484..e093edc76 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -833,7 +833,7 @@ def __init__( if isinstance(mesh, trimesh.primitives.Primitive): self._mesh = mesh.to_mesh() elif isinstance(mesh, trimesh.base.Trimesh): - self._mesh = mesh.copy() + self._mesh = mesh.copy(include_visual=False) else: raise TypeError( f"Got unexpected mesh parameter of type {type(mesh).__name__}" From 4bde5a5a39974eee787ccc80b4727e88ec12acfb Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 19:07:32 -0800 Subject: [PATCH 14/22] avoid redundant scale computations for fixed shapes --- src/scenic/core/object_types.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index f42cb9ca0..a2dfa7b69 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1331,16 +1331,24 @@ def _corners2D(self): @cached_property def occupiedSpace(self): """A region representing the space this object occupies""" - shape = self.shape + scaledShape = self._scaledShape + if scaledShape: + mesh = scaledShape.mesh + dimensions = None # mesh does not need to be scaled + convex = scaledShape.isConvex + else: + mesh = self.shape.mesh + dimensions = (self.width, self.length, self.height) + convex = self.shape.isConvex return MeshVolumeRegion( - mesh=shape.mesh, - dimensions=(self.width, self.length, self.height), + mesh=mesh, + dimensions=dimensions, position=self.position, rotation=self.orientation, centerMesh=False, _internal=True, - _isConvex=shape.isConvex, - _scaledShape=self._scaledShape, + _isConvex=convex, + _scaledShape=scaledShape, ) @precomputed_property @@ -1350,6 +1358,7 @@ def _scaledShape(shape, width, length, height): dimensions=(width, length, height), centerMesh=False, _internal=True, + _isConvex=shape.isConvex, ) @property From e9a8e1ec52d80c26767825369b4c6fa8937bd5d3 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sun, 8 Dec 2024 20:43:05 -0800 Subject: [PATCH 15/22] reuse occupiedSpace for fixed objects --- src/scenic/core/object_types.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index a2dfa7b69..4f821bce4 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1331,6 +1331,9 @@ def _corners2D(self): @cached_property def occupiedSpace(self): """A region representing the space this object occupies""" + if self._sampleParent and self._sampleParent._hasStaticBounds: + return self._sampleParent.occupiedSpace + scaledShape = self._scaledShape if scaledShape: mesh = scaledShape.mesh From ca71a2ea09a2753239798723e7dadee2d6b28a13 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Mon, 9 Dec 2024 12:38:58 -0800 Subject: [PATCH 16/22] compute MeshRegion.mesh lazily --- src/scenic/core/regions.py | 104 +++++++++++++++++++++---------------- tests/core/test_regions.py | 2 +- 2 files changed, 61 insertions(+), 45 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index e093edc76..943382b7a 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -829,34 +829,12 @@ def __init__( if isLazy(self): return - # Convert extract mesh - if isinstance(mesh, trimesh.primitives.Primitive): - self._mesh = mesh.to_mesh() - elif isinstance(mesh, trimesh.base.Trimesh): - self._mesh = mesh.copy(include_visual=False) - else: - raise TypeError( - f"Got unexpected mesh parameter of type {type(mesh).__name__}" - ) - - # Center mesh unless disabled - if centerMesh: - self.mesh.vertices -= self.mesh.bounding_box.center_mass - # Apply scaling, rotation, and translation, if any - if self.dimensions is not None: - scale = numpy.array(self.dimensions) / self.mesh.extents - else: - scale = None if self.rotation is not None: angles = self.rotation._trimeshEulerAngles() else: angles = None - self._transform = compose_matrix( - scale=scale, angles=angles, translate=self.position - ) self._rigidTransform = compose_matrix(angles=angles, translate=self.position) - self.mesh.apply_transform(self._transform) self.orientation = orientation @@ -941,10 +919,41 @@ def evaluateInner(self, context): ) ## API Methods ## - @property + @cached_property @distributionFunction def mesh(self): - return self._mesh + mesh = self._mesh + + # Convert/extract mesh + if isinstance(mesh, trimesh.primitives.Primitive): + mesh = mesh.to_mesh() + elif isinstance(mesh, trimesh.base.Trimesh): + mesh = mesh.copy(include_visual=False) + else: + raise TypeError( + f"Got unexpected mesh parameter of type {type(mesh).__name__}" + ) + + # Center mesh unless disabled + if self.centerMesh: + mesh.vertices -= mesh.bounding_box.center_mass + + # Apply scaling, rotation, and translation, if any + if self.dimensions is not None: + scale = numpy.array(self.dimensions) / mesh.extents + else: + scale = None + if self.rotation is not None: + angles = self.rotation._trimeshEulerAngles() + else: + angles = None + self._transform = compose_matrix( + scale=scale, angles=angles, translate=self.position + ) + mesh.apply_transform(self._transform) + + self._mesh = mesh + return mesh @distributionFunction def projectVector(self, point, onDirection): @@ -1090,6 +1099,7 @@ def __init__( super().__init__(*args, **kwargs) self._isConvex = _isConvex self._scaledShape = _scaledShape + self._num_samples = None if isLazy(self): return @@ -1107,19 +1117,6 @@ def __init__( " Consider using scenic.core.utils.repairMesh." ) - # Compute how many samples are necessary to achieve 99% probability - # of success when rejection sampling volume. - volume = _scaledShape._mesh.volume if _scaledShape else self._mesh.volume - p_volume = volume / self._mesh.bounding_box.volume - - if p_volume > 0.99: - self.num_samples = 1 - else: - self.num_samples = math.ceil(min(1e6, max(1, math.log(0.01, 1 - p_volume)))) - - # Always try to take at least 8 samples to avoid surface point total rejections - self.num_samples = max(self.num_samples, 8) - # Property testing methods # @distributionFunction def intersects(self, other, triedReversed=False): @@ -1135,8 +1132,8 @@ def intersects(self, other, triedReversed=False): # Check if bounding boxes intersect. If not, volumes cannot intersect. # For bounding boxes to intersect there must be overlap of the bounds # in all 3 dimensions. - bounds = self._mesh.bounds - obounds = other._mesh.bounds + bounds = self.mesh.bounds + obounds = other.mesh.bounds range_overlaps = ( (bounds[0, dim] <= obounds[1, dim]) and (obounds[0, dim] <= bounds[1, dim]) @@ -1262,7 +1259,7 @@ def containsPoint(self, point): return self.distanceTo(point) <= self.tolerance def _containsPointExact(self, point): - return self._mesh.contains([point])[0] + return self.mesh.contains([point])[0] @distributionFunction def containsObject(self, obj): @@ -1809,6 +1806,25 @@ def getVolumeRegion(self): """Returns this object, as it is already a MeshVolumeRegion""" return self + @property + def num_samples(self): + if self._num_samples is not None: + return self._num_samples + + # Compute how many samples are necessary to achieve 99% probability + # of success when rejection sampling volume. + volume = self._scaledShape.mesh.volume if self._scaledShape else self.mesh.volume + p_volume = volume / self.mesh.bounding_box.volume + + if p_volume > 0.99: + num_samples = 1 + else: + num_samples = math.ceil(min(1e6, max(1, math.log(0.01, 1 - p_volume)))) + + # Always try to take at least 8 samples to avoid surface point total rejections + self._num_samples = max(num_samples, 8) + return self._num_samples + @cached_property def _candidatePoint(self): # Use precomputed point if available (transformed appropriately) @@ -1818,7 +1834,7 @@ def _candidatePoint(self): return numpy.dot(self._rigidTransform, homog)[:3] # Use center of mass if it's contained - mesh = self._mesh + mesh = self.mesh com = mesh.bounding_box.center_mass if mesh.contains([com])[0]: return com @@ -1852,9 +1868,9 @@ def _candidateRadii(self): # Compute inradius and circumradius w.r.t. the candidate point point = self._candidatePoint - pq = trimesh.proximity.ProximityQuery(self._mesh) + pq = trimesh.proximity.ProximityQuery(self.mesh) inradius = abs(pq.signed_distance([point])[0]) - circumradius = numpy.max(numpy.linalg.norm(self._mesh.vertices - point, axis=1)) + circumradius = numpy.max(numpy.linalg.norm(self.mesh.vertices - point, axis=1)) return inradius, circumradius @cached_property @@ -1873,7 +1889,7 @@ def _fclData(self): trans = fcl.Transform(self.rotation.r.as_matrix(), numpy.array(self.position)) return geom, trans - mesh = self._mesh + 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) diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index e6029fb35..f8eae9643 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -348,7 +348,7 @@ def test_mesh_candidatePoint(): r = BoxRegion(dimensions=(d, d, d)).difference( BoxRegion(dimensions=(d - 1, d - 1, d - 1)) ) - r.num_samples = 8 # ensure sampling won't yield a good point + r._num_samples = 8 # ensure sampling won't yield a good point regions.append(r) bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4) From 3c516852605eb4ae7e26eac5a817fd9bde32200d Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Mon, 9 Dec 2024 14:08:53 -0800 Subject: [PATCH 17/22] do circumradius check before computing bounding boxes; skip inradius check unless precomputed --- src/scenic/core/object_types.py | 6 +- src/scenic/core/regions.py | 150 ++++++++++++++++---------------- src/scenic/core/shapes.py | 10 ++- src/scenic/core/utils.py | 34 ++++++++ tests/core/test_regions.py | 25 +++++- 5 files changed, 145 insertions(+), 80 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 4f821bce4..a49ac062c 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1334,15 +1334,16 @@ def occupiedSpace(self): if self._sampleParent and self._sampleParent._hasStaticBounds: return self._sampleParent.occupiedSpace + shape = self.shape scaledShape = self._scaledShape if scaledShape: mesh = scaledShape.mesh dimensions = None # mesh does not need to be scaled convex = scaledShape.isConvex else: - mesh = self.shape.mesh + mesh = shape.mesh dimensions = (self.width, self.length, self.height) - convex = self.shape.isConvex + convex = shape.isConvex return MeshVolumeRegion( mesh=mesh, dimensions=dimensions, @@ -1351,6 +1352,7 @@ def occupiedSpace(self): centerMesh=False, _internal=True, _isConvex=convex, + _shape=shape, _scaledShape=scaledShape, ) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 943382b7a..3c1c54b22 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -57,7 +57,13 @@ ) from scenic.core.lazy_eval import isLazy, valueInContext from scenic.core.type_support import toOrientation, toScalar, toVector -from scenic.core.utils import cached, cached_method, cached_property, unifyMesh +from scenic.core.utils import ( + cached, + cached_method, + cached_property, + findMeshInteriorPoint, + unifyMesh, +) from scenic.core.vectors import ( Orientation, OrientedVector, @@ -807,7 +813,7 @@ def __init__( # Copy parameters self._mesh = mesh self.dimensions = None if dimensions is None else toVector(dimensions) - self.position = None if position is None else toVector(position) + self.position = Vector(0, 0, 0) if position is None else toVector(position) self.rotation = None if rotation is None else toOrientation(rotation) self.orientation = None if orientation is None else toDistribution(orientation) self.tolerance = tolerance @@ -939,17 +945,6 @@ def mesh(self): mesh.vertices -= mesh.bounding_box.center_mass # Apply scaling, rotation, and translation, if any - if self.dimensions is not None: - scale = numpy.array(self.dimensions) / mesh.extents - else: - scale = None - if self.rotation is not None: - angles = self.rotation._trimeshEulerAngles() - else: - angles = None - self._transform = compose_matrix( - scale=scale, angles=angles, translate=self.position - ) mesh.apply_transform(self._transform) self._mesh = mesh @@ -1016,6 +1011,19 @@ def AABB(self): tuple(self.mesh.bounds[1]), ) + @cached_property + def _transform(self): + if self.dimensions is not None: + scale = numpy.array(self.dimensions) / self._mesh.extents + else: + scale = None + if self.rotation is not None: + angles = self.rotation._trimeshEulerAngles() + else: + angles = None + transform = compose_matrix(scale=scale, angles=angles, translate=self.position) + return transform + @cached_property def _boundingPolygonHull(self): assert not isLazy(self) @@ -1093,11 +1101,13 @@ def __init__( *args, _internal=False, _isConvex=None, + _shape=None, _scaledShape=None, **kwargs, ): super().__init__(*args, **kwargs) self._isConvex = _isConvex + self._shape = _shape self._scaledShape = _scaledShape self._num_samples = None @@ -1129,6 +1139,31 @@ def intersects(self, other, triedReversed=False): """ if isinstance(other, MeshVolumeRegion): # PASS 1 + # Check if the centers of the regions are far enough apart that the regions + # cannot overlap. This check only requires the circumradius of each region, + # which we can often precompute without explicitly constructing the mesh. + center_distance = numpy.linalg.norm(self.position - other.position) + if center_distance > self._circumradius + other._circumradius: + return False + + # PASS 1.5 + # If precomputed inradii are available, check if the volumes are close enough + # to ensure a collision. (While we're at it, check circumradii too.) + if self._scaledShape and other._scaledShape: + s_point = self._interiorPoint + s_inradius, s_circumradius = self._interiorPointRadii + o_point = other._interiorPoint + o_inradius, o_circumradius = other._interiorPointRadii + + point_distance = numpy.linalg.norm(s_point - o_point) + + if point_distance < s_inradius + o_inradius: + return True + + if point_distance > s_circumradius + o_circumradius: + return False + + # PASS 2 # Check if bounding boxes intersect. If not, volumes cannot intersect. # For bounding boxes to intersect there must be overlap of the bounds # in all 3 dimensions. @@ -1144,29 +1179,6 @@ def intersects(self, other, triedReversed=False): if not bb_overlap: return False - # PASS 2 - # Compute inradius and circumradius for a candidate point in each region, - # and compute the inradius and circumradius of each point. If the candidate - # points are closer than the sum of the inradius values, they must intersect. - # If the candidate points are farther apart than the sum of the circumradius - # values, they can't intersect. - - # Get a candidate point from each mesh and the corresponding - # inradius/circumradius around that point. - s_point = self._candidatePoint - s_inradius, s_circumradius = self._candidateRadii - o_point = other._candidatePoint - o_inradius, o_circumradius = other._candidateRadii - - # Get the distance between the two points and check for mandatory or impossible collision. - point_distance = numpy.linalg.norm(s_point - o_point) - - if point_distance < s_inradius + o_inradius: - return True - - if point_distance > s_circumradius + o_circumradius: - return False - # PASS 3 # Use FCL to check for intersection between the surfaces. # If the surfaces collide, that implies a collision of the volumes. @@ -1185,13 +1197,14 @@ def intersects(self, other, triedReversed=False): return surface_collision # PASS 4 - # If we have 2 candidate points and both regions have only one body, - # we can just check if either region contains the candidate point of the - # other. (This is because we previously ruled out surface intersections) + # If both regions have only one body, we can just check if either region + # contains an arbitrary interior point of the other. (This is because we + # previously ruled out surface intersections) if self._bodyCount == 1 and other._bodyCount == 1: - return self._containsPointExact(o_point) or other._containsPointExact( - s_point - ) + overlap = self._containsPointExact( + other._interiorPoint + ) or other._containsPointExact(self._interiorPoint) + return overlap # PASS 5 # Compute intersection and check if it's empty. Expensive but guaranteed @@ -1826,48 +1839,37 @@ def num_samples(self): return self._num_samples @cached_property - def _candidatePoint(self): + def _circumradius(self): + if self._scaledShape: + return self._scaledShape._circumradius + if self._shape: + scale = max(self.dimensions) if self.dimensions else 1 + return scale * self._shape._circumradius + + return numpy.max(numpy.linalg.norm(self.mesh.vertices, axis=1)) + + @cached_property + def _interiorPoint(self): # Use precomputed point if available (transformed appropriately) if self._scaledShape: - raw = self._scaledShape._candidatePoint + raw = self._scaledShape._interiorPoint homog = numpy.append(raw, [1]) return numpy.dot(self._rigidTransform, homog)[:3] + if self._shape: + raw = self._shape._interiorPoint + homog = numpy.append(raw, [1]) + return numpy.dot(self._transform, homog)[:3] - # Use center of mass if it's contained - mesh = self.mesh - com = mesh.bounding_box.center_mass - if mesh.contains([com])[0]: - return com - - # Try sampling a point inside the volume - samples = trimesh.sample.volume_mesh(mesh, self.num_samples) - if samples.size > 0: - return samples[0] - - # If all else fails, take a point from the surface and move inward - surfacePt, index = list(zip(*mesh.sample(1, return_index=True)))[0] - inward = -mesh.face_normals[index] - startPt = surfacePt + 1e-6 * inward - hits, _, _ = mesh.ray.intersects_location( - ray_origins=[startPt], - ray_directions=[inward], - multiple_hits=False, - ) - if hits.size > 0: - endPt = hits[0] - midPt = (surfacePt + endPt) / 2.0 - if mesh.contains([midPt])[0]: - return midPt - return surfacePt + return findMeshInteriorPoint(self.mesh, num_samples=self.num_samples) @cached_property - def _candidateRadii(self): + def _interiorPointRadii(self): # Use precomputed radii if available if self._scaledShape: - return self._scaledShape._candidateRadii + return self._scaledShape._interiorPointRadii - # Compute inradius and circumradius w.r.t. the candidate point - point = self._candidatePoint + # Compute inradius and circumradius w.r.t. the point + point = self._interiorPoint pq = trimesh.proximity.ProximityQuery(self.mesh) inradius = abs(pq.signed_distance([point])[0]) circumradius = numpy.max(numpy.linalg.norm(self.mesh.vertices - point, axis=1)) diff --git a/src/scenic/core/shapes.py b/src/scenic/core/shapes.py index 2a57c1f37..6dc224388 100644 --- a/src/scenic/core/shapes.py +++ b/src/scenic/core/shapes.py @@ -11,7 +11,7 @@ ) from scenic.core.type_support import toOrientation -from scenic.core.utils import cached_property, unifyMesh +from scenic.core.utils import cached_property, findMeshInteriorPoint, unifyMesh from scenic.core.vectors import Orientation ################################################################################################### @@ -64,6 +64,14 @@ def mesh(self): def isConvex(self): pass + @cached_property + def _circumradius(self): + return numpy.max(numpy.linalg.norm(self.mesh.vertices, axis=1)) + + @cached_property + def _interiorPoint(self): + return findMeshInteriorPoint(self.mesh) + ################################################################################################### # 3D Shape Classes diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 4549afdad..638f58b47 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -307,6 +307,40 @@ def repairMesh(mesh, pitch=(1 / 2) ** 6, verbose=True): raise ValueError("Mesh could not be repaired.") +def findMeshInteriorPoint(mesh, num_samples=None): + # Use center of mass if it's contained + com = mesh.bounding_box.center_mass + if mesh.contains([com])[0]: + return com + + # Try sampling a point inside the volume + if num_samples is None: + p_volume = mesh.volume / mesh.bounding_box.volume + if p_volume > 0.99: + num_samples = 1 + else: + num_samples = math.ceil(min(1e6, max(1, math.log(0.01, 1 - p_volume)))) + samples = trimesh.sample.volume_mesh(mesh, num_samples) + if samples.size > 0: + return samples[0] + + # If all else fails, take a point from the surface and move inward + surfacePt, index = list(zip(*mesh.sample(1, return_index=True)))[0] + inward = -mesh.face_normals[index] + startPt = surfacePt + 1e-6 * inward + hits, _, _ = mesh.ray.intersects_location( + ray_origins=[startPt], + ray_directions=[inward], + multiple_hits=False, + ) + if hits.size > 0: + endPt = hits[0] + midPt = (surfacePt + endPt) / 2.0 + if mesh.contains([midPt])[0]: + return midPt + return surfacePt + + class DefaultIdentityDict: """Dictionary which is the identity map by default. diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index f8eae9643..580224508 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -9,6 +9,7 @@ from scenic.core.distributions import RandomControlFlowError, Range from scenic.core.object_types import Object, OrientedPoint from scenic.core.regions import * +from scenic.core.shapes import MeshShape from scenic.core.vectors import Orientation, VectorField from tests.utils import deprecationTest, sampleSceneFrom @@ -339,7 +340,21 @@ def test_mesh_intersects(): assert not r1.getSurfaceRegion().intersects(r2.getSurfaceRegion()) -def test_mesh_candidatePoint(): +def test_mesh_circumradius(getAssetPath): + r1 = BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)) + bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4) + r2 = MeshVolumeRegion(r1.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r1) + planePath = getAssetPath("meshes/classic_plane.obj.bz2") + r3 = MeshVolumeRegion.fromFile(planePath, dimensions=(20, 20, 10)) + shape = MeshShape.fromFile(planePath) + r4 = MeshVolumeRegion(shape.mesh, dimensions=(0.5, 2, 1.5), rotation=bo, _shape=shape) + for reg in (r1, r2, r3, r4): + pos = reg.position + d = 2.01 * reg._circumradius + assert SpheroidRegion(dimensions=(d, d, d), position=pos).containsRegion(reg) + + +def test_mesh_interiorPoint(): regions = [ BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)), BoxRegion().difference(BoxRegion(dimensions=(0.1, 0.1, 0.1))), @@ -355,11 +370,15 @@ def test_mesh_candidatePoint(): r2 = MeshVolumeRegion(r.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r) regions.append(r2) + shape = MeshShape(BoxRegion(dimensions=(1, 2, 3)).mesh) + r3 = MeshVolumeRegion(shape.mesh, position=(-10, -5, 30), rotation=bo, _shape=shape) + regions.append(r3) + for reg in regions: - cp = reg._candidatePoint + cp = reg._interiorPoint # N.B. _containsPointExact can fail with embreex installed! assert reg.containsPoint(cp) - inr, circumr = reg._candidateRadii + inr, circumr = reg._interiorPointRadii d = 1.99 * inr assert reg.containsRegion(SpheroidRegion(dimensions=(d, d, d), position=cp)) d = 2.01 * circumr From 93cc3ab2a475de7580cd87323706c35c95ce8134 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Mon, 9 Dec 2024 14:21:51 -0800 Subject: [PATCH 18/22] skip bounding box check if meshes are lazy --- src/scenic/core/regions.py | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 3c1c54b22..c6b5b33f4 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1146,7 +1146,7 @@ def intersects(self, other, triedReversed=False): if center_distance > self._circumradius + other._circumradius: return False - # PASS 1.5 + # PASS 2A # If precomputed inradii are available, check if the volumes are close enough # to ensure a collision. (While we're at it, check circumradii too.) if self._scaledShape and other._scaledShape: @@ -1163,26 +1163,31 @@ def intersects(self, other, triedReversed=False): if point_distance > s_circumradius + o_circumradius: return False - # PASS 2 - # Check if bounding boxes intersect. If not, volumes cannot intersect. + # PASS 2B + # If precomputed geometry is not available, compute the bounding boxes + # (requiring that we construct the meshes, if they were previously lazy; + # hence we only do this check if we'll be constructing meshes anyway). # For bounding boxes to intersect there must be overlap of the bounds # in all 3 dimensions. - bounds = self.mesh.bounds - obounds = other.mesh.bounds - range_overlaps = ( - (bounds[0, dim] <= obounds[1, dim]) - and (obounds[0, dim] <= bounds[1, dim]) - for dim in range(3) - ) - bb_overlap = all(range_overlaps) + else: + bounds = self.mesh.bounds + obounds = other.mesh.bounds + range_overlaps = ( + (bounds[0, dim] <= obounds[1, dim]) + and (obounds[0, dim] <= bounds[1, dim]) + for dim in range(3) + ) + bb_overlap = all(range_overlaps) - if not bb_overlap: - return False + if not bb_overlap: + return False # PASS 3 # Use FCL 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) From 568a6e3b5c92a439aecf703029b07d4dfe50088a Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Tue, 10 Dec 2024 20:46:52 -0800 Subject: [PATCH 19/22] fix transforms of precomputed Shape data; cache MultiPoint of vertices --- src/scenic/core/regions.py | 33 ++++++++++++++++++++++-- src/scenic/core/shapes.py | 5 ++++ tests/core/test_regions.py | 52 +++++++++++++++++++++++++++++++++++++- 3 files changed, 87 insertions(+), 3 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index c6b5b33f4..33fc987f8 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1013,6 +1013,10 @@ def AABB(self): @cached_property def _transform(self): + """Transform from input mesh to final mesh. + + :meta private: + """ if self.dimensions is not None: scale = numpy.array(self.dimensions) / self._mesh.extents else: @@ -1024,9 +1028,33 @@ def _transform(self): transform = compose_matrix(scale=scale, angles=angles, translate=self.position) return transform + @cached_property + def _shapeTransform(self): + """Transform from Shape mesh (scaled to unit dimensions) to final mesh. + + :meta private: + """ + if self.dimensions is not None: + scale = numpy.array(self.dimensions) + else: + scale = self._mesh.extents + if self.rotation is not None: + angles = self.rotation._trimeshEulerAngles() + else: + angles = None + transform = compose_matrix(scale=scale, angles=angles, translate=self.position) + return transform + @cached_property def _boundingPolygonHull(self): assert not isLazy(self) + if self._shape: + raw = self._shape._multipoint + tr = self._shapeTransform + matrix = numpy.concatenate((tr[0:3, 0:3].flatten(), tr[0:3, 3])) + transformed = shapely.affinity.affine_transform(raw, matrix) + return transformed.convex_hull + return shapely.multipoints(self.mesh.vertices).convex_hull @cached_property @@ -1848,7 +1876,8 @@ def _circumradius(self): if self._scaledShape: return self._scaledShape._circumradius if self._shape: - scale = max(self.dimensions) if self.dimensions else 1 + dims = self.dimensions or self._mesh.extents + scale = max(dims) return scale * self._shape._circumradius return numpy.max(numpy.linalg.norm(self.mesh.vertices, axis=1)) @@ -1863,7 +1892,7 @@ def _interiorPoint(self): if self._shape: raw = self._shape._interiorPoint homog = numpy.append(raw, [1]) - return numpy.dot(self._transform, homog)[:3] + return numpy.dot(self._shapeTransform, homog)[:3] return findMeshInteriorPoint(self.mesh, num_samples=self.num_samples) diff --git a/src/scenic/core/shapes.py b/src/scenic/core/shapes.py index 6dc224388..db1c04deb 100644 --- a/src/scenic/core/shapes.py +++ b/src/scenic/core/shapes.py @@ -3,6 +3,7 @@ from abc import ABC, abstractmethod import numpy +import shapely import trimesh from trimesh.transformations import ( concatenate_matrices, @@ -72,6 +73,10 @@ def _circumradius(self): def _interiorPoint(self): return findMeshInteriorPoint(self.mesh) + @cached_property + def _multipoint(self): + return shapely.multipoints(self.mesh.vertices) + ################################################################################################### # 3D Shape Classes diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index 580224508..03b832e6e 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -14,6 +14,11 @@ from tests.utils import deprecationTest, sampleSceneFrom +def assertPolygonsEqual(p1, p2, prec=1e-6): + assert p1.difference(p2).area == pytest.approx(0, abs=prec) + assert p2.difference(p1).area == pytest.approx(0, abs=prec) + + def sample_ignoring_rejections(region, num_samples): samples = [] for _ in range(num_samples): @@ -340,7 +345,23 @@ def test_mesh_intersects(): assert not r1.getSurfaceRegion().intersects(r2.getSurfaceRegion()) -def test_mesh_circumradius(getAssetPath): +def test_mesh_boundingPolygon(getAssetPath, pytestconfig): + r = BoxRegion(dimensions=(8, 6, 2)).difference(BoxRegion(dimensions=(2, 2, 3))) + bp = r.boundingPolygon + poly = shapely.geometry.Polygon( + [(-4, 3), (4, 3), (4, -3), (-4, -3)], [[(-1, 1), (1, 1), (1, -1), (-1, -1)]] + ) + assertPolygonsEqual(bp.polygons, poly) + + shape = MeshShape(BoxRegion(dimensions=(1, 2, 3)).mesh) + o = Orientation.fromEuler(0, 0, math.pi / 4) + r = MeshVolumeRegion(shape.mesh, dimensions=(2, 4, 2), rotation=o, _shape=shape) + bp = r.boundingPolygon + sr2 = math.sqrt(2) + poly = shapely.geometry.Polygon([(-sr2, 2), (sr2, 2), (sr2, -2), (-sr2, -2)]) + assertPolygonsEqual(bp.polygons, poly) + + samples = 50 if pytestconfig.getoption("--fast") else 200 r1 = BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)) bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4) r2 = MeshVolumeRegion(r1.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r1) @@ -349,6 +370,29 @@ def test_mesh_circumradius(getAssetPath): shape = MeshShape.fromFile(planePath) r4 = MeshVolumeRegion(shape.mesh, dimensions=(0.5, 2, 1.5), rotation=bo, _shape=shape) for reg in (r1, r2, r3, r4): + bp = reg.boundingPolygon + pts = trimesh.sample.volume_mesh(reg.mesh, samples) + assert all(bp.containsPoint(pt) for pt in pts) + + +def test_mesh_circumradius(getAssetPath): + r1 = BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)) + + bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4) + r2 = MeshVolumeRegion(r1.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r1) + + planePath = getAssetPath("meshes/classic_plane.obj.bz2") + r3 = MeshVolumeRegion.fromFile(planePath, dimensions=(20, 20, 10)) + + shape = MeshShape.fromFile(planePath) + r4 = MeshVolumeRegion(shape.mesh, dimensions=(0.5, 2, 1.5), rotation=bo, _shape=shape) + + r = BoxRegion(dimensions=(1, 2, 3)).difference(BoxRegion(dimensions=(0.5, 1, 1))) + shape = MeshShape(r.mesh) + scaled = MeshVolumeRegion(shape.mesh, dimensions=(6, 5, 4)).mesh + r5 = MeshVolumeRegion(scaled, position=(-10, -5, 30), rotation=bo, _shape=shape) + + for reg in (r1, r2, r3, r4, r5): pos = reg.position d = 2.01 * reg._circumradius assert SpheroidRegion(dimensions=(d, d, d), position=pos).containsRegion(reg) @@ -374,6 +418,12 @@ def test_mesh_interiorPoint(): r3 = MeshVolumeRegion(shape.mesh, position=(-10, -5, 30), rotation=bo, _shape=shape) regions.append(r3) + r = BoxRegion(dimensions=(1, 2, 3)).difference(BoxRegion(dimensions=(0.5, 1, 1))) + shape = MeshShape(r.mesh) + scaled = MeshVolumeRegion(shape.mesh, dimensions=(0.1, 0.1, 0.1)).mesh + r4 = MeshVolumeRegion(scaled, position=(-10, -5, 30), rotation=bo, _shape=shape) + regions.append(r4) + for reg in regions: cp = reg._interiorPoint # N.B. _containsPointExact can fail with embreex installed! From ca508ea56800239a5b102ed30444a2c8c6afec86 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Wed, 11 Dec 2024 20:07:33 -0800 Subject: [PATCH 20/22] minor tweaks & extra tests to cover new lines --- src/scenic/core/regions.py | 9 ++++++--- src/scenic/core/utils.py | 5 ++++- tests/core/test_regions.py | 12 ++++++++++++ tests/core/test_shapes.py | 15 ++++++++++++++- 4 files changed, 36 insertions(+), 5 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 33fc987f8..c541106c2 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -835,6 +835,11 @@ def __init__( if isLazy(self): return + if not isinstance(mesh, (trimesh.primitives.Primitive, trimesh.base.Trimesh)): + raise TypeError( + f"Got unexpected mesh parameter of type {type(mesh).__name__}" + ) + # Apply scaling, rotation, and translation, if any if self.rotation is not None: angles = self.rotation._trimeshEulerAngles() @@ -936,9 +941,7 @@ def mesh(self): elif isinstance(mesh, trimesh.base.Trimesh): mesh = mesh.copy(include_visual=False) else: - raise TypeError( - f"Got unexpected mesh parameter of type {type(mesh).__name__}" - ) + assert False, f"mesh of invalid type {type(mesh).__name__}" # Center mesh unless disabled if self.centerMesh: diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 638f58b47..68ab3c374 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -338,7 +338,10 @@ def findMeshInteriorPoint(mesh, num_samples=None): midPt = (surfacePt + endPt) / 2.0 if mesh.contains([midPt])[0]: return midPt - return surfacePt + + # Should never get here with reasonable geometry, but we return a surface + # point just in case. + return surfacePt # pragma: no cover class DefaultIdentityDict: diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index 03b832e6e..5228e20d9 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -295,6 +295,13 @@ def test_mesh_region_fromFile(getAssetPath): ) +def test_mesh_region_invalid_mesh(): + with pytest.raises(TypeError): + MeshVolumeRegion(42) + with pytest.raises(TypeError): + MeshSurfaceRegion(42) + + def test_mesh_volume_region_zero_dimension(): for dims in ((0, 1, 1), (1, 0, 1), (1, 1, 0)): with pytest.raises(ValueError): @@ -354,6 +361,11 @@ def test_mesh_boundingPolygon(getAssetPath, pytestconfig): assertPolygonsEqual(bp.polygons, poly) shape = MeshShape(BoxRegion(dimensions=(1, 2, 3)).mesh) + r = MeshVolumeRegion(shape.mesh, dimensions=(2, 4, 2), _shape=shape) + bp = r.boundingPolygon + poly = shapely.geometry.Polygon([(-1, 2), (1, 2), (1, -2), (-1, -2)]) + assertPolygonsEqual(bp.polygons, poly) + o = Orientation.fromEuler(0, 0, math.pi / 4) r = MeshVolumeRegion(shape.mesh, dimensions=(2, 4, 2), rotation=o, _shape=shape) bp = r.boundingPolygon diff --git a/tests/core/test_shapes.py b/tests/core/test_shapes.py index 95e257f8f..a27fd6b3d 100644 --- a/tests/core/test_shapes.py +++ b/tests/core/test_shapes.py @@ -3,7 +3,8 @@ import pytest -from scenic.core.shapes import BoxShape, MeshShape +from scenic.core.regions import BoxRegion +from scenic.core.shapes import BoxShape, CylinderShape, MeshShape def test_shape_fromFile(getAssetPath): @@ -21,3 +22,15 @@ def test_invalid_dimension(badDim): BoxShape(dimensions=dims) with pytest.raises(ValueError): BoxShape(scale=badDim) + + +def test_circumradius(): + s = CylinderShape(dimensions=(3, 1, 17)) # dimensions don't matter + assert s._circumradius == pytest.approx(math.sqrt(2) / 2) + + +def test_interiorPoint(): + s = MeshShape(BoxRegion().difference(BoxRegion(dimensions=(0.1, 0.1, 0.1))).mesh) + pt = s._interiorPoint + assert all(-0.5 <= coord <= 0.5 for coord in pt) + assert not all(-0.05 <= coord <= 0.05 for coord in pt) From 8ecc9bcd2589aa021327b3b9eea1fb920d8d372a Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Thu, 12 Dec 2024 20:12:24 -0800 Subject: [PATCH 21/22] minor fix; more tests --- src/scenic/core/regions.py | 1 - tests/core/test_regions.py | 41 ++++++++++++++++++++++++++++++++------ 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index c541106c2..6047ec6ab 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -950,7 +950,6 @@ def mesh(self): # Apply scaling, rotation, and translation, if any mesh.apply_transform(self._transform) - self._mesh = mesh return mesh @distributionFunction diff --git a/tests/core/test_regions.py b/tests/core/test_regions.py index 5228e20d9..3b860e762 100644 --- a/tests/core/test_regions.py +++ b/tests/core/test_regions.py @@ -9,7 +9,7 @@ from scenic.core.distributions import RandomControlFlowError, Range from scenic.core.object_types import Object, OrientedPoint from scenic.core.regions import * -from scenic.core.shapes import MeshShape +from scenic.core.shapes import ConeShape, MeshShape from scenic.core.vectors import Orientation, VectorField from tests.utils import deprecationTest, sampleSceneFrom @@ -19,6 +19,10 @@ def assertPolygonsEqual(p1, p2, prec=1e-6): assert p2.difference(p1).area == pytest.approx(0, abs=prec) +def assertPolygonCovers(p1, p2, prec=1e-6): + assert p2.difference(p1).area == pytest.approx(0, abs=prec) + + def sample_ignoring_rejections(region, num_samples): samples = [] for _ in range(num_samples): @@ -359,12 +363,15 @@ def test_mesh_boundingPolygon(getAssetPath, pytestconfig): [(-4, 3), (4, 3), (4, -3), (-4, -3)], [[(-1, 1), (1, 1), (1, -1), (-1, -1)]] ) assertPolygonsEqual(bp.polygons, poly) + poly = shapely.geometry.Polygon([(-4, 3), (4, 3), (4, -3), (-4, -3)]) + assertPolygonsEqual(r._boundingPolygonHull, poly) shape = MeshShape(BoxRegion(dimensions=(1, 2, 3)).mesh) r = MeshVolumeRegion(shape.mesh, dimensions=(2, 4, 2), _shape=shape) bp = r.boundingPolygon poly = shapely.geometry.Polygon([(-1, 2), (1, 2), (1, -2), (-1, -2)]) assertPolygonsEqual(bp.polygons, poly) + assertPolygonsEqual(r._boundingPolygonHull, poly) o = Orientation.fromEuler(0, 0, math.pi / 4) r = MeshVolumeRegion(shape.mesh, dimensions=(2, 4, 2), rotation=o, _shape=shape) @@ -372,19 +379,41 @@ def test_mesh_boundingPolygon(getAssetPath, pytestconfig): sr2 = math.sqrt(2) poly = shapely.geometry.Polygon([(-sr2, 2), (sr2, 2), (sr2, -2), (-sr2, -2)]) assertPolygonsEqual(bp.polygons, poly) + assertPolygonsEqual(r._boundingPolygonHull, poly) samples = 50 if pytestconfig.getoption("--fast") else 200 - r1 = BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)) + regions = [] + # Convex + r = BoxRegion(dimensions=(1, 2, 3), position=(4, 5, 6)) + regions.append(r) + # Convex, with scaledShape plus transform bo = Orientation.fromEuler(math.pi / 4, math.pi / 4, math.pi / 4) - r2 = MeshVolumeRegion(r1.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r1) + regions.append( + MeshVolumeRegion(r.mesh, position=(15, 20, 5), rotation=bo, _scaledShape=r) + ) + # Convex, with shape and scaledShape plus transform + s = MeshShape(r.mesh) + regions.append( + MeshVolumeRegion( + r.mesh, rotation=bo, position=(4, 5, 6), _shape=s, _scaledShape=r + ) + ) + # Not convex planePath = getAssetPath("meshes/classic_plane.obj.bz2") - r3 = MeshVolumeRegion.fromFile(planePath, dimensions=(20, 20, 10)) + regions.append(MeshVolumeRegion.fromFile(planePath, dimensions=(20, 20, 10))) + # Not convex, with shape plus transform shape = MeshShape.fromFile(planePath) - r4 = MeshVolumeRegion(shape.mesh, dimensions=(0.5, 2, 1.5), rotation=bo, _shape=shape) - for reg in (r1, r2, r3, r4): + regions.append( + MeshVolumeRegion(shape.mesh, dimensions=(0.5, 2, 1.5), rotation=bo, _shape=shape) + ) + for reg in regions: bp = reg.boundingPolygon pts = trimesh.sample.volume_mesh(reg.mesh, samples) assert all(bp.containsPoint(pt) for pt in pts) + bphull = reg._boundingPolygonHull + assertPolygonCovers(bphull, bp.polygons) + simple = shapely.multipoints(reg.mesh.vertices).convex_hull + assertPolygonsEqual(bphull, simple) def test_mesh_circumradius(getAssetPath): From d633f103d9c94ccda8e2cb7dd0887ff9a1eafb53 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sat, 11 Jan 2025 15:26:28 -0800 Subject: [PATCH 22/22] fix reproducibility issue --- src/scenic/core/utils.py | 7 +++- tests/syntax/test_distributions.py | 54 +++++++++++++++++++++++++----- 2 files changed, 51 insertions(+), 10 deletions(-) diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 68ab3c374..9817843f5 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -320,7 +320,12 @@ def findMeshInteriorPoint(mesh, num_samples=None): num_samples = 1 else: num_samples = math.ceil(min(1e6, max(1, math.log(0.01, 1 - p_volume)))) - samples = trimesh.sample.volume_mesh(mesh, num_samples) + + # Do the "random" number generation ourselves so that it's deterministic + # (this helps debugging and reproducibility) + rng = numpy.random.default_rng(49493130352093220597973654454967996892) + pts = (rng.random((num_samples, 3)) * mesh.extents) + mesh.bounds[0] + samples = pts[mesh.contains(pts)] if samples.size > 0: return samples[0] diff --git a/tests/syntax/test_distributions.py b/tests/syntax/test_distributions.py index c45572988..6b983c5d2 100644 --- a/tests/syntax/test_distributions.py +++ b/tests/syntax/test_distributions.py @@ -668,18 +668,54 @@ def test_reproducibility(): assert iterations == baseIterations +def test_reproducibility_lazy_interior(): + """Regression test for a reproducibility issue involving lazy region computations. + + In this test, an interior point of the objects' shape is computed on demand + during the first sample, then cached. The code for doing so used NumPy's PRNG, + meaning that a second sample with the same random seed could differ. + """ + scenario = compileScenic( + """ + import numpy + from scenic.core.distributions import distributionFunction + @distributionFunction + def gen(arg): + return numpy.random.random() + + region = BoxRegion().difference(BoxRegion(dimensions=(0.1, 0.1, 0.1))) + shape = MeshShape(region.mesh) # Shape which does not contain its center + other = new Object with shape shape + ego = new Object at (Range(0.9, 1.1), 0), with shape shape + param foo = ego intersects other # trigger computation of interior point + param bar = gen(ego) # generate number using NumPy's PRNG + """ + ) + seed = random.randint(0, 1000000000) + random.seed(seed) + numpy.random.seed(seed) + s1 = sampleScene(scenario, maxIterations=60) + random.seed(seed) + numpy.random.seed(seed) + s2 = sampleScene(scenario, maxIterations=60) + assert s1.params["bar"] == s2.params["bar"] + assert s1.egoObject.x == s2.egoObject.x + + @pytest.mark.slow def test_reproducibility_3d(): scenario = compileScenic( - "ego = new Object\n" - "workspace = Workspace(SpheroidRegion(dimensions=(25,15,10)))\n" - "region = BoxRegion(dimensions=(25,15,0.1))\n" - "obj_1 = new Object in workspace, facing Range(0, 360) deg, with width Range(0.5, 1), with length Range(0.5,1)\n" - "obj_2 = new Object in workspace, facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg)\n" - "obj_3 = new Object in workspace, on region\n" - "param foo = Uniform(1, 4, 9, 16, 25, 36)\n" - "x = Range(0, 1)\n" - "require x > 0.8" + """ + ego = new Object + workspace = Workspace(SpheroidRegion(dimensions=(5,5,5))) + region = BoxRegion(dimensions=(25,15,0.1)) + #obj_1 = new Object in workspace, facing Range(0, 360) deg, with width Range(0.5, 1), with length Range(0.5,1) + obj_2 = new Object in workspace, facing (Range(0, 360) deg, Range(0, 360) deg, Range(0, 360) deg) + #obj_3 = new Object in workspace, on region + param foo = ego intersects obj_2 + x = Range(0, 1) + require x > 0.8 + """ ) seeds = [random.randint(0, 100) for i in range(10)] for seed in seeds: