From ef9d735fa83b3d024d7615e6db8b2b5888d52a3f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 6 Nov 2024 16:49:04 -0800 Subject: [PATCH 001/134] Initial simulator stepping implementation. --- src/scenic/core/simulators.py | 351 +++++++++++++++++++++------------- 1 file changed, 223 insertions(+), 128 deletions(-) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 832b03632..18c1fa5c8 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -11,7 +11,8 @@ """ import abc -from collections import defaultdict +from collections import OrderedDict, defaultdict +from contextlib import contextmanager import enum import math import numbers @@ -135,9 +136,8 @@ def simulate( (rarely) and its security implications. Returns: - A `Simulation` object representing the completed simulation, or `None` if no - simulation satisfying the requirements could be found within - **maxIterations** iterations. + An initialized simulation, or `None` if no simulation satisfying + the requirements could be found within **maxIterations** iterations. Raises: SimulationCreationError: if an error occurred while trying to run a @@ -191,6 +191,53 @@ def simulate( ) return simulation + @contextmanager + def simulateStepped( + self, + scene, + maxSteps=None, + *, + name="SteppedSimulation", + timestep=None, + verbosity=None, + replay=None, + enableReplay=True, + enableDivergenceCheck=False, + divergenceTolerance=0, + continueAfterDivergence=False, + allowPickle=False, + ): + if self._destroyed: + raise RuntimeError( + "simulator cannot run additional simulations " + "(the destroy() method has already been called)" + ) + if verbosity is None: + verbosity = errors.verbosityLevel + + simulation = self.createSimulation( + scene, + maxSteps=maxSteps, + name=name, + verbosity=verbosity, + timestep=timestep, + replay=replay, + enableReplay=enableReplay, + enableDivergenceCheck=enableDivergenceCheck, + divergenceTolerance=divergenceTolerance, + continueAfterDivergence=continueAfterDivergence, + allowPickle=allowPickle, + ) + try: + yield simulation + except (RejectSimulationException, RejectionException, GuardViolation) as e: + # This simulation will be thrown out, but attach it to the exception + # to aid in debugging. + e.simulation = self + raise + finally: + simulation.cleanup() + def replay(self, scene, replay, **kwargs): """Replay a simulation. @@ -207,13 +254,15 @@ def _runSingleSimulation( if verbosity >= 2: print(f" Starting simulation {name}...") try: - simulation = self.createSimulation( + with self.simulateStepped( scene, maxSteps=maxSteps, name=name, verbosity=verbosity, **kwargs, - ) + ) as simulation: + simulation._run() + except (RejectSimulationException, RejectionException, GuardViolation) as e: if verbosity >= 2: print( @@ -339,11 +388,14 @@ def __init__( self.currentTime = 0 self.timestep = 1 if timestep is None else float(timestep) self.verbosity = verbosity + self.maxSteps = maxSteps self.name = name self.worker_num = 0 self.actionSequence = [] + self._cleaned = False + # Prepare to save or load a replay. self.initializeReplay(replay, enableReplay, enableDivergenceCheck, allowPickle) self.divergenceTolerance = divergenceTolerance @@ -356,153 +408,192 @@ def __init__( import scenic.syntax.veneer as veneer veneer.beginSimulation(self) - dynamicScenario = self.scene.dynamicScenario + self.dynamicScenario = self.scene.dynamicScenario # Create objects and perform simulator-specific initialization. self.setup() # Initialize the top-level dynamic scenario. - dynamicScenario._start() + self.dynamicScenario._start() # Update all objects in case the simulator has adjusted any dynamic # properties during setup. self.updateObjects() - # Run the simulation. - terminationType, terminationReason = self._run(dynamicScenario, maxSteps) + # Set terminationType and terminationReason to default None + self.terminationType = None + self.terminationReason = None - # Stop all remaining scenarios. - # (and reject if some 'require eventually' condition was never satisfied) - for scenario in tuple(reversed(veneer.runningScenarios)): - scenario._stop("simulation terminated") - - # Record finally-recorded values. - values = dynamicScenario._evaluateRecordedExprs(RequirementType.recordFinal) - for name, val in values.items(): - self.records[name] = val - - # Package up simulation results into a compact object. - result = SimulationResult( - self.trajectory, - self.actionSequence, - terminationType, - terminationReason, - self.records, - ) - self.result = result except (RejectSimulationException, RejectionException, GuardViolation) as e: # This simulation will be thrown out, but attach it to the exception # to aid in debugging. + self.cleanup() e.simulation = self raise - finally: - self.destroy() - for obj in self.objects: - disableDynamicProxyFor(obj) - for agent in self.agents: - if agent.behavior and agent.behavior._isRunning: - agent.behavior._stop() - # If the simulation was terminated by an exception (including rejections), - # some scenarios may still be running; we need to clean them up without - # checking their requirements, which could raise rejection exceptions. - for scenario in tuple(reversed(veneer.runningScenarios)): - scenario._stop("exception", quiet=True) - veneer.endSimulation(self) - - def _run(self, dynamicScenario, maxSteps): + + def _run(self): assert self.currentTime == 0 while True: - if self.verbosity >= 3: - print(f" Time step {self.currentTime}:") - - # Run compose blocks of compositional scenarios - # (and check if any requirements defined therein fail) - # N.B. if the top-level scenario completes, we don't immediately end - # the simulation since we need to check if any monitors reject first. - terminationReason = dynamicScenario._step() - terminationType = TerminationType.scenarioComplete - - # Record current state of the simulation - self.recordCurrentState() - - # Run monitors - newReason = dynamicScenario._runMonitors() - if newReason is not None: - terminationReason = newReason - terminationType = TerminationType.terminatedByMonitor - - # "Always" and scenario-level requirements have been checked; - # now safe to terminate if the top-level scenario has finished, - # a monitor requested termination, or we've hit the timeout - if terminationReason is not None: - return terminationType, terminationReason - terminationReason = dynamicScenario._checkSimulationTerminationConditions() - if terminationReason is not None: - return TerminationType.simulationTerminationCondition, terminationReason - if maxSteps and self.currentTime >= maxSteps: - return TerminationType.timeLimit, f"reached time limit ({maxSteps} steps)" - - # Clear lastActions for all objects - for obj in self.objects: - obj.lastActions = tuple() - - # Update agents with any objects that now have behaviors (and are not already agents) - self.agents += [ - obj for obj in self.objects if obj.behavior and obj not in self.agents - ] - - # Compute the actions of the agents in this time step - allActions = defaultdict(tuple) - schedule = self.scheduleForAgents() - if not set(self.agents) == set(schedule): - raise RuntimeError("Simulator schedule does not contain all agents") - for agent in schedule: - # If agent doesn't have a behavior right now, continue - if not agent.behavior: - continue - - # Run the agent's behavior to get its actions - actions = agent.behavior._step() - - # Handle pseudo-actions marking the end of a simulation/scenario - if isinstance(actions, _EndSimulationAction): - return TerminationType.terminatedByBehavior, str(actions) - elif isinstance(actions, _EndScenarioAction): - scenario = actions.scenario - if scenario._isRunning: - scenario._stop(actions) - if scenario is dynamicScenario: - # Top-level scenario was terminated, so whole simulation will end. - return TerminationType.terminatedByBehavior, str(actions) - actions = () - - # Check ordinary actions for compatibility - assert isinstance(actions, tuple) - if len(actions) == 1 and isinstance(actions[0], (list, tuple)): - actions = tuple(actions[0]) - if not self.actionsAreCompatible(agent, actions): - raise InvalidScenarioError( - f"agent {agent} tried incompatible action(s) {actions}" + self.advance() + + if self.terminationType: + return + + def advance(self): + if self.terminationType or self._cleaned: + raise TerminatedSimulationException() + + if self.verbosity >= 3: + print(f" Time step {self.currentTime}:") + + # Run compose blocks of compositional scenarios + # (and check if any requirements defined therein fail) + # N.B. if the top-level scenario completes, we don't immediately end + # the simulation since we need to check if any monitors reject first. + terminationReason = self.dynamicScenario._step() + terminationType = TerminationType.scenarioComplete + + # Record current state of the simulation + self.recordCurrentState() + + # Run monitors + newReason = self.dynamicScenario._runMonitors() + if newReason is not None: + terminationReason = newReason + terminationType = TerminationType.terminatedByMonitor + + # "Always" and scenario-level requirements have been checked; + # now safe to terminate if the top-level scenario has finished, + # a monitor requested termination, or we've hit the timeout + if terminationReason is not None: + return self.terminateSimulation(terminationType, terminationReason) + terminationReason = self.dynamicScenario._checkSimulationTerminationConditions() + if terminationReason is not None: + return self.terminateSimulation( + TerminationType.simulationTerminationCondition, terminationReason + ) + if self.maxSteps and self.currentTime >= self.maxSteps: + return self.terminateSimulation( + TerminationType.timeLimit, f"reached time limit ({self.maxSteps} steps)" + ) + + # Clear lastActions for all objects + for obj in self.objects: + obj.lastActions = tuple() + + # Update agents with any objects that now have behaviors (and are not already agents) + self.agents += [ + obj for obj in self.objects if obj.behavior and obj not in self.agents + ] + + # Compute the actions of the agents in this time step + allActions = defaultdict(tuple) + schedule = self.scheduleForAgents() + if not set(self.agents) == set(schedule): + raise RuntimeError("Simulator schedule does not contain all agents") + for agent in schedule: + # If agent doesn't have a behavior right now, continue + if not agent.behavior: + continue + # Run the agent's behavior to get its actions + actions = agent.behavior._step() + + # Handle pseudo-actions marking the end of a simulation/scenario + if isinstance(actions, _EndSimulationAction): + return self.terminateSimulation( + TerminationType.terminatedByBehavior, str(actions) + ) + elif isinstance(actions, _EndScenarioAction): + scenario = actions.scenario + if scenario._isRunning: + scenario._stop(actions) + if scenario is self.dynamicScenario: + # Top-level scenario was terminated, so whole simulation will end. + return self.terminateSimulation( + TerminationType.terminatedByBehavior, str(actions) ) + actions = () + + # Check ordinary actions for compatibility + assert isinstance(actions, tuple) + if len(actions) == 1 and isinstance(actions[0], (list, tuple)): + actions = tuple(actions[0]) + if not self.actionsAreCompatible(agent, actions): + raise InvalidScenarioError( + f"agent {agent} tried incompatible action(s) {actions}" + ) - # Save actions for execution below - allActions[agent] = actions + # Save actions for execution below + allActions[agent] = actions - # Log lastActions + # Log lastActions + agent.lastActions = actions + + # Execute the actions + if self.verbosity >= 3: + for agent, actions in allActions.items(): + print(f" Agent {agent} takes action(s) {actions}") agent.lastActions = actions + self.actionSequence.append(allActions) + self.executeActions(allActions) - # Execute the actions - if self.verbosity >= 3: - for agent, actions in allActions.items(): - print(f" Agent {agent} takes action(s) {actions}") - self.actionSequence.append(allActions) - self.executeActions(allActions) + # Run the simulation for a single step and read its state back into Scenic + self.step() + self.currentTime += 1 + self.updateObjects() - # Run the simulation for a single step and read its state back into Scenic - self.step() - self.currentTime += 1 - self.updateObjects() + def terminateSimulation(self, terimnationType, terminationReason): + import scenic.syntax.veneer as veneer + + # Log terminationType and terminationReason + self.terminationType = terimnationType + self.terminationReason = terminationReason + + # Stop all remaining scenarios. + # (and reject if some 'require eventually' condition was never satisfied) + for scenario in tuple(reversed(veneer.runningScenarios)): + scenario._stop("simulation terminated") + + # Record finally-recorded values. + values = self.dynamicScenario._evaluateRecordedExprs(RequirementType.recordFinal) + for name, val in values.items(): + self.records[name] = val + + # Package up simulation results into a compact object. + result = SimulationResult( + self.trajectory, + self.actionSequence, + self.terminationType, + self.terminationReason, + self.records, + ) + self.result = result + + self.cleanup() + + def cleanup(self): + # No need to repeat cleanup if we've already done it + if self._cleaned: + return + + # Remember that we have cleaned up. + self._cleaned = True + + import scenic.syntax.veneer as veneer + + self.destroy() + for obj in self.objects: + disableDynamicProxyFor(obj) + for agent in self.agents: + if agent.behavior and agent.behavior._isRunning: + agent.behavior._stop() + # If the simulation was terminated by an exception (including rejections), + # some scenarios may still be running; we need to clean them up without + # checking their requirements, which could raise rejection exceptions. + for scenario in tuple(reversed(veneer.runningScenarios)): + scenario._stop("exception", quiet=True) + veneer.endSimulation(self) def setup(self): """Set up the simulation to run in the simulator. @@ -911,3 +1002,7 @@ def __init__(self, trajectory, actions, terminationType, terminationReason, reco self.terminationType = terminationType self.terminationReason = str(terminationReason) self.records = dict(records) + + +class TerminatedSimulationException(Exception): + pass From a452067a6131eb27a9ff97d8b1248d01f7f68c35 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 6 Nov 2024 17:07:06 -0800 Subject: [PATCH 002/134] Added stepped simulation test. --- src/scenic/core/simulators.py | 18 +++++------------- tests/core/test_simulators.py | 30 +++++++++++++++++++++++++++++- 2 files changed, 34 insertions(+), 14 deletions(-) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 18c1fa5c8..57574767d 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -420,10 +420,6 @@ def __init__( # properties during setup. self.updateObjects() - # Set terminationType and terminationReason to default None - self.terminationType = None - self.terminationReason = None - except (RejectSimulationException, RejectionException, GuardViolation) as e: # This simulation will be thrown out, but attach it to the exception # to aid in debugging. @@ -437,11 +433,11 @@ def _run(self): while True: self.advance() - if self.terminationType: + if self.result: return def advance(self): - if self.terminationType or self._cleaned: + if self.result or self._cleaned: raise TerminatedSimulationException() if self.verbosity >= 3: @@ -543,13 +539,9 @@ def advance(self): self.currentTime += 1 self.updateObjects() - def terminateSimulation(self, terimnationType, terminationReason): + def terminateSimulation(self, terminationType, terminationReason): import scenic.syntax.veneer as veneer - # Log terminationType and terminationReason - self.terminationType = terimnationType - self.terminationReason = terminationReason - # Stop all remaining scenarios. # (and reject if some 'require eventually' condition was never satisfied) for scenario in tuple(reversed(veneer.runningScenarios)): @@ -564,8 +556,8 @@ def terminateSimulation(self, terimnationType, terminationReason): result = SimulationResult( self.trajectory, self.actionSequence, - self.terminationType, - self.terminationReason, + terminationType, + terminationReason, self.records, ) self.result = result diff --git a/tests/core/test_simulators.py b/tests/core/test_simulators.py index 149c1cad1..5358a8cec 100644 --- a/tests/core/test_simulators.py +++ b/tests/core/test_simulators.py @@ -1,6 +1,11 @@ import pytest -from scenic.core.simulators import DummySimulation, DummySimulator, Simulation +from scenic.core.simulators import ( + DummySimulation, + DummySimulator, + Simulation, + TerminatedSimulationException, +) from tests.utils import compileScenic, sampleResultFromScene, sampleSceneFrom @@ -35,6 +40,29 @@ def test_simulator_destruction(): assert "destroy() called twice" in str(e) +def test_simulator_stepped(): + simulator = DummySimulator() + scene = sampleSceneFrom("ego = new Object") + + with simulator.simulateStepped(scene, maxSteps=5) as simulation: + while simulation.result is None: + simulation.advance() + + assert simulation.result is not None + assert simulation.currentTime == 5 + + # advance() should do nothing but raise an exception + # if the simulation is already terminated + with pytest.raises(TerminatedSimulationException): + simulation.advance() + + assert simulation.currentTime == 5 + + # Ensure all values are preserved after leaving the context manager + assert simulation.result is not None + assert simulation.currentTime == 5 + + def test_simulator_set_property(): class TestSimulation(DummySimulation): def createObjectInSimulator(self, obj): From 1cd2aa65bce0bdc1a3b3d71247da141107e8949f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sun, 16 Nov 2025 20:55:51 -0800 Subject: [PATCH 003/134] Work on OpenScenarioXML Export. --- src/scenic/core/serialization.py | 76 ++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index a7c52367a..e473032b3 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -364,3 +364,79 @@ def readStr(stream): Serializer.addCodec(str, writeStr, readStr) + +from scenariogeneration import ScenarioGenerator, xosc + + +def toOpenScenario( + scenario, + scene, + simulationResult, + wheelbaseRatio=0.6, + maxSteeringAngle=0.523598775598, + wheelDiameter=0.8, + trackWidth=1.68, + groundClearance=0.4, + maxSpeed=69, + maxAcceleration=10, + maxDeceleration=10, +): + # Create catalog + xosc_catalog = xosc.Catalog() + + # Extract map + assert "map" in scenario.params + map_path = scenario.params["map"] + xosc_road = xosc.RoadNetwork(roadfile=map_path) + + # Create entitities + entities = xosc.Entities() + xosc_objects = [] + for obj_i, obj in enumerate(scene.objects): + veh_name = obj.name if hasattr(obj, "name") else f"Vehicle_{obj_i}" + # NOTE: XOSC coordinate system swaps X and Y compared to Scenic. + veh_bb = xosc.BoundingBox( + obj.length, + obj.width, + obj.height, + 0.5 * wheelbaseRatio * obj.length, + 0, + obj.height / 2, + ) + veh_fa = xosc.Axle( + maxSteeringAngle, + wheelDiameter, + trackWidth, + wheelbaseRatio * obj.length, + groundClearance, + ) + veh_ra = xosc.Axle( + maxSteeringAngle, wheelDiameter, trackWidth, 0, groundClearance + ) + xosc_veh = xosc.Vehicle( + name=veh_name, + vehicle_type=xosc.VehicleCategory.car, + boundingbox=veh_bb, + frontaxle=veh_fa, + rearaxle=veh_ra, + max_speed=maxSpeed, + max_acceleration=maxAcceleration, + max_deceleration=maxDeceleration, + mass=None, + model3d=None, + max_acceleration_rate=None, + max_deceleration_rate=None, + role=None, + ) + xosc_objects.append(xosc_veh) + entities.add_scenario_object(veh_name, xosc_veh) + + # Create init + init = xosc.Init() + + for xosc_obj in xosc_objects: + breakpoint() + obj_init_action = xosc.TeleportAction(xosc.LanePosition(25, 0, -1, 1)) + init.add_init_action(xosc_obj.name, obj_init_action) + + assert False From 8c056c2a07e915da540073a82836d7fc8c67cfb8 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 18 Nov 2025 15:09:52 -0800 Subject: [PATCH 004/134] Initial OpenScenarioXML export implementation. --- src/scenic/core/regions.py | 3 + src/scenic/core/serialization.py | 110 ++++++++++++++++++++++++++++--- src/scenic/core/simulators.py | 14 +++- 3 files changed, 117 insertions(+), 10 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 38e876d01..00cb8bca5 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -3695,6 +3695,9 @@ def distanceTo(self, point) -> float: dist2D = self.lineString.distance(makeShapelyPoint(point)) return math.hypot(dist2D, point.z) + def distanceAlong(self, point) -> float: + return shapely.line_locate_point(self.lineString, makeShapelyPoint(point)) + def projectVector(self, point, onDirection): raise TypeError('PolylineRegion does not support projection using "on"') diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index e473032b3..e869b2358 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -7,6 +7,7 @@ import io import math +import os import pickle import struct import types @@ -372,6 +373,8 @@ def toOpenScenario( scenario, scene, simulationResult, + mapPath=None, + scenarioName="ScenicScenario", wheelbaseRatio=0.6, maxSteeringAngle=0.523598775598, wheelDiameter=0.8, @@ -384,22 +387,27 @@ def toOpenScenario( # Create catalog xosc_catalog = xosc.Catalog() + # Create parameters + xosc_paramdec = xosc.ParameterDeclarations() + # Extract map assert "map" in scenario.params - map_path = scenario.params["map"] + map_path = mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) xosc_road = xosc.RoadNetwork(roadfile=map_path) + # network = scenario.dynamicScenario._dummyNamespace["network"] + # Create entitities entities = xosc.Entities() - xosc_objects = [] + xosc_objects = {} for obj_i, obj in enumerate(scene.objects): - veh_name = obj.name if hasattr(obj, "name") else f"Vehicle_{obj_i}" + veh_name = obj.name if hasattr(obj, "name") else f"Vehicle{obj_i}" # NOTE: XOSC coordinate system swaps X and Y compared to Scenic. veh_bb = xosc.BoundingBox( obj.length, obj.width, obj.height, - 0.5 * wheelbaseRatio * obj.length, + 0, 0, obj.height / 2, ) @@ -428,15 +436,99 @@ def toOpenScenario( max_deceleration_rate=None, role=None, ) - xosc_objects.append(xosc_veh) + xosc_objects[obj] = xosc_veh entities.add_scenario_object(veh_name, xosc_veh) # Create init init = xosc.Init() - for xosc_obj in xosc_objects: - breakpoint() - obj_init_action = xosc.TeleportAction(xosc.LanePosition(25, 0, -1, 1)) + for obj, xosc_obj in xosc_objects.items(): + init_position = xosc.WorldPosition(x=obj.x, y=obj.y, z=obj.z, h=obj.heading) + obj_init_action = xosc.TeleportAction(init_position) init.add_init_action(xosc_obj.name, obj_init_action) - assert False + # Dynamics + xosc_act = xosc.Act( + "MainAct", + xosc.ValueTrigger( + "StartSimulation", + 0, + xosc.ConditionEdge.none, + xosc.SimulationTimeCondition(0, xosc.Rule.greaterThan), + ), + ) + + for obj_i, (obj, xosc_obj) in enumerate(xosc_objects.items()): + action_times = [] + action_positions = [] + for t, states in enumerate(simulationResult.trajectory): + state_position = states.positions[obj_i] + state_orientation = states.orientations[obj_i].yaw + action_times.append(simulationResult.timestep * t) + pos = xosc.WorldPosition( + x=state_position.x, + y=state_position.y, + z=state_position.z, + h=state_orientation, + ) + action_positions.append(pos) + + polyline = xosc.Polyline(time=action_times, positions=action_positions) + trajectory = xosc.Trajectory(name=f"Trajectory_{xosc_obj.name}", closed=False) + trajectory.add_shape(polyline) + + traj_action = xosc.FollowTrajectoryAction( + trajectory=trajectory, + following_mode=xosc.FollowingMode.position, + reference_domain=xosc.ReferenceContext.absolute, + scale=1, + offset=0, + ) + + event = xosc.Event(f"Event_{xosc_obj.name}", xosc.Priority.override) + event.add_trigger( + xosc.ValueTrigger( + f"TimeTrigger_{xosc_obj.name}_{t}", + 0, + xosc.ConditionEdge.none, + xosc.SimulationTimeCondition(0, xosc.Rule.greaterThan), + ) + ) + event.add_action(f"Action_{xosc_obj.name}", action=traj_action) + + maneuver = xosc.Maneuver("Maneuver_{xosc_obj.name}") + maneuver.add_event(event) + + manuever_group = xosc.ManeuverGroup(f"ManeuverGroup_{xosc_obj.name}") + manuever_group.add_maneuver(maneuver) + manuever_group.add_actor(xosc_obj.name) + + xosc_act.add_maneuver_group(manuever_group) + + # Create storyboard + xosc_sb = xosc.StoryBoard( + init, + xosc.ValueTrigger( + "StopSimulation", + 0, + xosc.ConditionEdge.rising, + xosc.SimulationTimeCondition( + simulationResult.currentRealTime, xosc.Rule.greaterThan + ), + "stop", + ), + ) + xosc_sb.add_act(xosc_act) + + # Create scenario + xosc_scenario = xosc.Scenario( + scenarioName, + "Scenic", + xosc_paramdec, + entities=entities, + storyboard=xosc_sb, + roadnetwork=xosc_road, + catalog=xosc_catalog, + ) + + return xosc_scenario diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 3e9c0308f..f662c451d 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -811,7 +811,19 @@ def currentState(self): The default implementation returns a tuple of the positions of all objects. """ - return tuple(obj.position for obj in self.objects) + + class SimulationState(tuple): + def __new__(cls, positions, orientations): + return super().__new__(cls, positions) + + def __init__(self, positions, orientations): + self.positions = positions + self.orientations = orientations + + positions = tuple(obj.position for obj in self.objects) + orientation = tuple(obj.orientation for obj in self.objects) + + return SimulationState(positions, orientation) @property def currentRealTime(self): From 048d3f00d9229e9c165703df33e8829e9178d184 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 19 Nov 2025 09:28:24 -0800 Subject: [PATCH 005/134] Tidying up --- src/scenic/core/regions.py | 3 --- src/scenic/core/serialization.py | 9 ++++++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 00cb8bca5..38e876d01 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -3695,9 +3695,6 @@ def distanceTo(self, point) -> float: dist2D = self.lineString.distance(makeShapelyPoint(point)) return math.hypot(dist2D, point.z) - def distanceAlong(self, point) -> float: - return shapely.line_locate_point(self.lineString, makeShapelyPoint(point)) - def projectVector(self, point, onDirection): raise TypeError('PolylineRegion does not support projection using "on"') diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index e869b2358..006fdf925 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -11,6 +11,7 @@ import pickle import struct import types +import warnings from scenic.core.distributions import Samplable, needsSampling from scenic.core.utils import DefaultIdentityDict @@ -395,12 +396,14 @@ def toOpenScenario( map_path = mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) xosc_road = xosc.RoadNetwork(roadfile=map_path) - # network = scenario.dynamicScenario._dummyNamespace["network"] - # Create entitities entities = xosc.Entities() xosc_objects = {} for obj_i, obj in enumerate(scene.objects): + if not hasattr(obj, "isVehicle") or not obj.isVehicle: + warnings.warn("Non-vehicle object {} is ignored.") + continue + veh_name = obj.name if hasattr(obj, "name") else f"Vehicle{obj_i}" # NOTE: XOSC coordinate system swaps X and Y compared to Scenic. veh_bb = xosc.BoundingBox( @@ -488,7 +491,7 @@ def toOpenScenario( event = xosc.Event(f"Event_{xosc_obj.name}", xosc.Priority.override) event.add_trigger( xosc.ValueTrigger( - f"TimeTrigger_{xosc_obj.name}_{t}", + f"TimeTrigger_{xosc_obj.name}", 0, xosc.ConditionEdge.none, xosc.SimulationTimeCondition(0, xosc.Rule.greaterThan), From 07e773b6380ca8b8732ff00d616df2f7defa238f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 19 Nov 2025 18:30:50 -0800 Subject: [PATCH 006/134] Scene sampling parallelization prototype. --- src/scenic/__init__.py | 1 + src/scenic/__main__.py | 3 +- src/scenic/core/scenarios.py | 87 ++++++++++++++++++++++++++------- src/scenic/core/utils.py | 38 ++++++++++++++ src/scenic/syntax/translator.py | 17 ++++++- 5 files changed, 124 insertions(+), 22 deletions(-) diff --git a/src/scenic/__init__.py b/src/scenic/__init__.py index ac15ea073..fe8fbeb72 100644 --- a/src/scenic/__init__.py +++ b/src/scenic/__init__.py @@ -2,6 +2,7 @@ import scenic.core.errors as _errors from scenic.core.errors import setDebuggingOptions +from scenic.core.utils import setSeed from scenic.syntax.translator import scenarioFromFile, scenarioFromString _errors.showInternalBacktrace = False # see comment in errors module diff --git a/src/scenic/__main__.py b/src/scenic/__main__.py index 05f527fba..076ace8c9 100644 --- a/src/scenic/__main__.py +++ b/src/scenic/__main__.py @@ -185,8 +185,7 @@ if args.verbosity >= 1: print(f"Using random seed = {args.seed}") - random.seed(args.seed) - numpy.random.seed(args.seed) + scenic.setSeed(args.seed) # Load scenario from file if args.verbosity >= 1: diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index fa93d454b..7bf3a6e06 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -3,9 +3,11 @@ import dataclasses import io import itertools +import multiprocessing import random import sys import time +import warnings import numpy import trimesh @@ -38,6 +40,7 @@ ) from scenic.core.sample_checking import BasicChecker, WeightedAcceptanceChecker from scenic.core.serialization import Serializer, dumpAsScenicCode +from scenic.core.utils import generateInnerBatchHelper from scenic.core.vectors import Vector # Global params @@ -315,6 +318,7 @@ def __init__( self.dependencies = ( self._instances + paramDeps + tuple(requirementDeps) + tuple(behaviorDeps) ) + self._scenarioCreationData = None # Setup the default checker self.defaultRequirements = self.generateDefaultRequirements() @@ -400,11 +404,16 @@ def generate(self, maxIterations=2000, verbosity=0, feedback=None): Raises: `RejectionException`: if no valid sample is found in **maxIterations** iterations. """ - scenes, iterations = self.generateBatch(1, maxIterations, verbosity, feedback) - return scenes[0], iterations + return next(self.generateBatch(1, maxIterations, verbosity, feedback)) def generateBatch( - self, numScenes, maxIterations=float("inf"), verbosity=0, feedback=None + self, + numScenes, + maxIterations=float("inf"), + verbosity=0, + feedback=None, + numWorkers=0, + mute=True, ): """Sample several `Scene` objects from this scenario. @@ -416,29 +425,71 @@ def generateBatch( verbosity (int): Verbosity level. feedback (float): Feedback to pass to external samplers doing active sampling. See :mod:`scenic.core.external_params`. + numWorkers (int): The number of workers to be used when generating scenes. If numWorkers + is 0, scenes will be generated in the main process. + mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. Returns: - A pair with a list of the sampled `Scene` objects and the total number - of iterations used. + An iterable of pairs with a sampled `Scene` and the number of iterations used for that scene. Raises: `RejectionException`: if not enough valid samples are found in **maxIterations** iterations. """ - totalIterations = 0 - scenes = [] + if numWorkers == 0: + totalIterations = 0 + + for _ in range(numScenes): + try: + remainingIts = maxIterations - totalIterations + scene, iterations = self._generateInner( + remainingIts, verbosity, feedback + ) + totalIterations += iterations + yield (scene, iterations) + except RejectionException: + raise RejectionException( + f"failed to generate scenario in {maxIterations} iterations" + ) + else: + if maxIterations != float("inf"): + raise RuntimeError("maxIterations not supported for parallel sampling.") - for _ in range(numScenes): - try: - remainingIts = maxIterations - totalIterations - scene, iterations = self._generateInner(remainingIts, verbosity, feedback) - scenes.append(scene) - totalIterations += iterations - except RejectionException: - raise RejectionException( - f"failed to generate scenario in {maxIterations} iterations" - ) + if feedback is not None: + raise RuntimeError("Feedback not supported for parallel sampling.") + + if verbosity > 0: + warnings.warn("Verbosity > 0 ignored during parallel sampling") + + # Initialize queues and lock + seedQueue = multiprocessing.Queue() + for _ in range(numScenes): + seedQueue.put(random.getrandbits(32)) - return scenes, totalIterations + sceneQueue = multiprocessing.Queue() + + # Initialize processes + params = (self._scenarioCreationData, seedQueue, sceneQueue, mute) + processes = [ + multiprocessing.Process(target=generateInnerBatchHelper, args=params) + for _ in range(numWorkers) + ] + try: + # Prepare process pool + for process in processes: + process.start() + + for _ in range(numScenes): + sceneBytes, iterations = sceneQueue.get() + scene = self.sceneFromBytes(sceneBytes, verify=False) + yield (scene, iterations) + + finally: + # Close processes and queues + for process in processes: + process.terminate() + + seedQueue.close() + sceneQueue.close() def _generateInner(self, maxIterations, verbosity, feedback): # choose which custom requirements will be enforced for this sample diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 9817843f5..63cee18d1 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -4,9 +4,11 @@ import collections from contextlib import contextmanager import functools +import io import itertools import math import os +import random import signal from subprocess import CalledProcessError import sys @@ -393,3 +395,39 @@ def get_type_hints(obj, globalns=None, localns=None): wrapped = wrapped.__wrapped__ globalns = getattr(wrapped, "__globals__", {}) return typing.get_type_hints(obj, globalns, localns) + + +def setSeed(seed): + random.seed(seed) + numpy.random.seed(seed) + + +def generateInnerBatchHelper(scenarioCreationData, seedQueue, sceneQueue, mute): + if mute: + sys.stdout = open(os.devnull, "w") + sys.stderr = open(os.devnull, "w") + + from scenic.syntax.translator import _scenarioFromStream + + stream = io.BytesIO(scenarioCreationData["streamLines"]) + + scenario = _scenarioFromStream( + stream=stream, + compileOptions=scenarioCreationData["compileOptions"], + filename=scenarioCreationData["filename"], + scenario=scenarioCreationData["scenario"], + path=scenarioCreationData["path"], + _cacheImports=False, + ) + + while True: + seed = seedQueue.get() + + setSeed(seed) + + scene, iterations = scenario._generateInner( + maxIterations=float("inf"), verbosity=0, feedback=None + ) + sceneBytes = scenario.sceneToBytes(scene) + + sceneQueue.put((sceneBytes, iterations)) diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index 994e65b8b..e563e6fa8 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -77,7 +77,7 @@ def hash(self): if isinstance(value, (int, float, str)): stream.write(str(value).encode()) else: - stream.write([0]) + stream.write(str([0]).encode()) if self.scenario: stream.write(self.scenario.encode()) # We can't use `hash` because it is not deterministic @@ -165,6 +165,17 @@ def _scenarioFromStream( behavior as importing a Python module. See `purgeModulesUnsafeToCache` for a more detailed discussion of the internals behind this. """ + # Backup stream and parameters + streamLines = stream.read() + scenarioCreationData = { + "streamLines": streamLines, + "compileOptions": compileOptions, + "filename": filename, + "scenario": scenario, + "path": path, + } + stream = io.BytesIO(streamLines) + # Compile the code as if it were a top-level module oldModules = list(sys.modules.keys()) try: @@ -174,7 +185,9 @@ def _scenarioFromStream( if not _cacheImports: purgeModulesUnsafeToCache(oldModules) # Construct a Scenario from the resulting namespace - return constructScenarioFrom(namespace, scenario) + scenario = constructScenarioFrom(namespace, scenario) + scenario._scenarioCreationData = scenarioCreationData + return scenario @contextmanager From 99f4ac0a3932c79300554a92e261956ba48f1de2 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 21 Nov 2025 12:50:54 -0800 Subject: [PATCH 007/134] Scene sampling parallelization. --- src/scenic/core/scenarios.py | 116 +++++++++++++- src/scenic/core/utils.py | 9 +- tests/core/test_scenarios.py | 62 ++++++++ .../benchmark_parallelization.py | 84 ++++++++++ .../benchmarks/adjacentOpposingPair.scenic | 6 + .../benchmarks/badlyParkedCarPullingIn.scenic | 30 ++++ .../benchmarks/bypassing_03.scenic | 106 +++++++++++++ .../benchmarks/city_intersection.scenic | 131 +++++++++++++++ .../benchmarks/enclosed_occluded.scenic | 20 +++ .../benchmarks/enclosed_visible.scenic | 22 +++ .../benchmarks/fully_occluded.scenic | 15 ++ .../benchmarks/fully_visible.scenic | 12 ++ .../benchmarks/narrowGoalNew.scenic | 102 ++++++++++++ .../benchmarks/narrowGoalOld.scenic | 83 ++++++++++ .../benchmarks/pedestrian_02.scenic | 89 +++++++++++ .../parallelization/benchmarks/vacuum.scenic | 149 ++++++++++++++++++ 16 files changed, 1024 insertions(+), 12 deletions(-) create mode 100644 tools/benchmarking/parallelization/benchmark_parallelization.py create mode 100644 tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/city_intersection.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/fully_visible.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic create mode 100644 tools/benchmarking/parallelization/benchmarks/vacuum.scenic diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index 7bf3a6e06..362aaa715 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -404,7 +404,16 @@ def generate(self, maxIterations=2000, verbosity=0, feedback=None): Raises: `RejectionException`: if no valid sample is found in **maxIterations** iterations. """ - return next(self.generateBatch(1, maxIterations, verbosity, feedback)) + scenes, totalIterations = self.generateBatch( + numScenes=1, + maxIterations=maxIterations, + verbosity=verbosity, + feedback=feedback, + numWorkers=0, + mute=False, + ) + assert len(scenes) == 1 + return (scenes[0], totalIterations) def generateBatch( self, @@ -414,6 +423,57 @@ def generateBatch( feedback=None, numWorkers=0, mute=True, + deterministic=True, + serialized=False, + ): + """Sample several `Scene` objects from this scenario. + + For a description of how scene generation is done, see `scene generation`. + + Args: + numScenes (int): Number of scenes to generate. + maxIterations (int): Maximum number of rejection sampling iterations (over all scenes). + verbosity (int): Verbosity level. + feedback (float): Feedback to pass to external samplers doing active sampling. + See :mod:`scenic.core.external_params`. + numWorkers (int): The number of workers to be used when generating scenes. If numWorkers + is 0, scenes will be generated in the main process. + mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. + deterministic (bool): Whether or not scenes will be returned in a deterministic order. + serialized (bool): Whether or not to return scenes in a serialized format. + + Returns: + A pair with a list of the sampled `Scene` objects and the total number + of iterations used. + + Raises: + `RejectionException`: if not enough valid samples are found in **maxIterations** iterations. + """ + stream = self.generateStream( + numScenes=numScenes, + maxIterations=maxIterations, + verbosity=verbosity, + feedback=feedback, + numWorkers=numWorkers, + mute=mute, + deterministic=deterministic, + serialized=serialized, + ) + results_list = list(stream) + scenes = tuple(r[0] for r in results_list) + totalIterations = sum([r[1] for r in results_list]) + return (scenes, totalIterations) + + def generateStream( + self, + numScenes, + maxIterations=float("inf"), + verbosity=0, + feedback=None, + numWorkers=0, + mute=True, + deterministic=False, + serialized=False, ): """Sample several `Scene` objects from this scenario. @@ -428,6 +488,10 @@ def generateBatch( numWorkers (int): The number of workers to be used when generating scenes. If numWorkers is 0, scenes will be generated in the main process. mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. + deterministic (bool): Whether or not scenes will be returned in a deterministic order. + NOTE: Setting this to True may increase latency when waiting for the next `Scene` in + the stream. + serialized (bool): Whether or not to return scenes in a serialized format. Returns: An iterable of pairs with a sampled `Scene` and the number of iterations used for that scene. @@ -457,31 +521,67 @@ def generateBatch( if feedback is not None: raise RuntimeError("Feedback not supported for parallel sampling.") - if verbosity > 0: - warnings.warn("Verbosity > 0 ignored during parallel sampling") + # Initialize results tracking data + resultsList = [] + returnedResults = 0 # Initialize queues and lock seedQueue = multiprocessing.Queue() + seedHistory = {} + + def putSeed(): + newSeed = random.getrandbits(32) + seedQueue.put(newSeed) + seedHistory[newSeed] = len(seedHistory) + if deterministic: + resultsList.append(None) + for _ in range(numScenes): - seedQueue.put(random.getrandbits(32)) + putSeed() sceneQueue = multiprocessing.Queue() # Initialize processes - params = (self._scenarioCreationData, seedQueue, sceneQueue, mute) + params = (self._scenarioCreationData, seedQueue, sceneQueue, verbosity, mute) processes = [ multiprocessing.Process(target=generateInnerBatchHelper, args=params) for _ in range(numWorkers) ] + + # Initialized result management functions + def getResult(): + sceneBytes, resultIterations, resultSeed = sceneQueue.get() + resultScene = ( + sceneBytes + if serialized + else self.sceneFromBytes(sceneBytes, verify=False) + ) + return (resultScene, resultIterations), resultSeed + + def getNextResult(): + if not deterministic: + return getResult()[0] + + while True: + assert len(resultsList) > 0 + + if resultsList[0] is not None: + return resultsList.pop(0) + + result, resultSeed = getResult() + resultIndex = seedHistory[resultSeed] - returnedResults + assert resultsList[resultIndex] is None + resultsList[resultIndex] = result + + # Start sampling processes and yield samples try: # Prepare process pool for process in processes: process.start() for _ in range(numScenes): - sceneBytes, iterations = sceneQueue.get() - scene = self.sceneFromBytes(sceneBytes, verify=False) - yield (scene, iterations) + yield getNextResult() + returnedResults += 1 finally: # Close processes and queues diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 63cee18d1..632557641 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -402,7 +402,9 @@ def setSeed(seed): numpy.random.seed(seed) -def generateInnerBatchHelper(scenarioCreationData, seedQueue, sceneQueue, mute): +def generateInnerBatchHelper( + scenarioCreationData, seedQueue, sceneQueue, verbosity, mute +): if mute: sys.stdout = open(os.devnull, "w") sys.stderr = open(os.devnull, "w") @@ -422,12 +424,11 @@ def generateInnerBatchHelper(scenarioCreationData, seedQueue, sceneQueue, mute): while True: seed = seedQueue.get() - setSeed(seed) scene, iterations = scenario._generateInner( - maxIterations=float("inf"), verbosity=0, feedback=None + maxIterations=float("inf"), verbosity=verbosity, feedback=None ) sceneBytes = scenario.sceneToBytes(scene) - sceneQueue.put((sceneBytes, iterations)) + sceneQueue.put((sceneBytes, iterations, seed)) diff --git a/tests/core/test_scenarios.py b/tests/core/test_scenarios.py index 548d5d146..e4365d462 100644 --- a/tests/core/test_scenarios.py +++ b/tests/core/test_scenarios.py @@ -1,6 +1,10 @@ +import random + import pytest from scenic.core.distributions import Range +from scenic.core.scenarios import Scene +from scenic.core.utils import setSeed from tests.utils import compileScenic @@ -59,3 +63,61 @@ def test_condition_scenario_params_2(): assert all(0.5 <= x <= 0.51 for x in xs) assert any(0.505 <= x for x in xs) assert any(x < 0.505 for x in xs) + + +def test_generateBatch(): + scenario = compileScenic( + """ + ego = new Object facing Range(0, 1) + require ego.heading > 0.5 + """ + ) + scenes, _ = scenario.generateBatch(2, numWorkers=2) + + assert all(isinstance(scene, Scene) for scene in scenes) + assert all(0 <= scene.objects[0].heading <= 1 for scene in scenes) + assert scenes[0].objects[0].heading != scenes[1].objects[0].heading + + +def test_generateBatch_serialized(): + scenario = compileScenic( + """ + ego = new Object facing Range(0, 1) + require ego.heading > 0.5 + """ + ) + scenesBytes, _ = scenario.generateBatch(2, numWorkers=2, serialized=True) + assert all(isinstance(b, bytes) for b in scenesBytes) + + scenes = [scenario.sceneFromBytes(b, verify=True) for b in scenesBytes] + assert all(isinstance(scene, Scene) for scene in scenes) + assert all(0 <= scene.objects[0].heading <= 1 for scene in scenes) + assert scenes[0].objects[0].heading != scenes[1].objects[0].heading + + +def test_generateStream_deterministic(): + seed = random.getrandbits(32) + + scenario = compileScenic( + """ + ego = new Object facing Range(0, 1) + require ego.heading > 0.5 + """ + ) + setSeed(seed) + streamA = tuple(scenario.generateStream(8, numWorkers=2, serialized=True)) + setSeed(seed) + streamB = tuple(scenario.generateStream(8, numWorkers=2, serialized=True)) + bytesSetA = {result[0] for result in streamA} + bytesSetB = {result[0] for result in streamB} + assert bytesSetA == bytesSetB + + setSeed(seed) + streamA = tuple( + scenario.generateStream(8, numWorkers=2, serialized=True, deterministic=True) + ) + setSeed(seed) + streamB = tuple( + scenario.generateStream(8, numWorkers=2, serialized=True, deterministic=True) + ) + assert streamA == streamB diff --git a/tools/benchmarking/parallelization/benchmark_parallelization.py b/tools/benchmarking/parallelization/benchmark_parallelization.py new file mode 100644 index 000000000..fec0f3191 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmark_parallelization.py @@ -0,0 +1,84 @@ +from pathlib import Path +import time +import warnings + +import scenic + +NUM_WORKERS = 8 +BENCHMARKS_BASE_PATH = (Path(__file__).resolve().parent / "benchmarks").resolve() +MAP_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent + / "assets" + / "maps" + / "CARLA" + / "Town05.xodr" +).resolve() +MESH_BASE_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent / "assets" / "meshes" +).resolve() + +BENCHMARKS = [ + ("adjacentOpposingPair.scenic", {"mode2D": True, "map": MAP_PATH}), + ("badlyParkedCarPullingIn.scenic", {"mode2D": True, "map": MAP_PATH}), + ("bypassing_03.scenic", {"mode2D": True, "map": MAP_PATH}), + ("city_intersection.scenic", {"meshBasePath": MESH_BASE_PATH}), + # ("enclosed_occluded.scenic", {}), + ("enclosed_visible.scenic", {}), + ("fully_occluded.scenic", {"meshBasePath": MESH_BASE_PATH}), + ("fully_visible.scenic", {"meshBasePath": MESH_BASE_PATH}), + ("narrowGoalNew.scenic", {"meshBasePath": MESH_BASE_PATH}), + ("narrowGoalOld.scenic", {"mode2D": True, "map": MAP_PATH}), + ("pedestrian_02.scenic", {"mode2D": True, "map": MAP_PATH}), + ("vacuum.scenic", {"numToys": 0, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 1, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 2, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 4, "meshBasePath": MESH_BASE_PATH}), + ("vacuum.scenic", {"numToys": 8, "meshBasePath": MESH_BASE_PATH}), + # ("vacuum.scenic", {"numToys": 16, "meshBasePath": MESH_BASE_PATH}), +] + +NUM_SAMPLES = 128 + + +def run_benchmark(path, params): + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + for _ in range(NUM_SAMPLES): + scenario.generate(maxIterations=float("inf")) + + +def run_benchmark_parallel(path, params, *, numWorkers): + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + scenario.generateBatch(NUM_SAMPLES, maxIterations=float("inf"), numWorkers=numWorkers) + + +if __name__ == "__main__": + print("Base Performance (`generate`, numWorkers=0):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark(*benchmark) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") + print() + print("Base + Overhead Performance (`generateBatch`, numWorkers=1):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=1) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") + print() + print("Batch Performance (`generateBatch`, numWorkers=8):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=8) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") diff --git a/tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic b/tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic new file mode 100644 index 000000000..055bc93fe --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/adjacentOpposingPair.scenic @@ -0,0 +1,6 @@ +model scenic.simulators.carla.model + +ego = new Car with visibleDistance 20 +c2 = new Car visible +c3 = new Car at c2 offset by Range(-10, 1) @ 0 +require abs(relative heading of c3 from c2) >= 150 deg \ No newline at end of file diff --git a/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic b/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic new file mode 100644 index 000000000..9e1b15b18 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic @@ -0,0 +1,30 @@ +param time_step = 1.0/10 + +model scenic.domains.driving.model + +behavior PullIntoRoad(): + while (distance from self to ego) > 15: + wait + do FollowLaneBehavior(laneToFollow=ego.lane) + +ego = new Car with behavior DriveAvoidingCollisions(avoidance_threshold=5) + +rightCurb = ego.laneGroup.curb +spot = new OrientedPoint on visible rightCurb +badAngle = Uniform(1.0, -1.0) * Range(10, 20) deg +parkedCar = new Car left of spot by 0.5, + facing badAngle relative to roadDirection, + with behavior PullIntoRoad + +require (distance to parkedCar) > 20 + +monitor StopAfterInteraction(): + for i in range(50): + wait + while ego.speed > 2: + wait + for i in range(50): + wait + terminate +require monitor StopAfterInteraction() +terminate after 15 seconds # in case ego never breaks diff --git a/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic b/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic new file mode 100644 index 000000000..ebbab8023 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic @@ -0,0 +1,106 @@ +""" +TITLE: Bypassing 03 +AUTHOR: Francis Indaheng, findaheng@berkeley.edu +DESCRIPTION: Ego vehicle performs a lane change to bypass a slow +adversary vehicle but cannot return to its original lane because +the adversary accelerates. Ego vehicle must then slow down to avoid +collision with leading vehicle in new lane. +SOURCE: NHSTA, #16 +""" + +################################# +# MAP AND MODEL # +################################# + +model scenic.simulators.carla.model + +################################# +# CONSTANTS # +################################# + +MODEL = 'vehicle.lincoln.mkz2017' + +param EGO_SPEED = VerifaiRange(7, 10) +param EGO_BRAKE = VerifaiRange(0.7, 1.0) + +param ADV_DIST = VerifaiRange(10, 15) +param ADV_INIT_SPEED = VerifaiRange(2, 4) +param ADV_END_SPEED = 2 * VerifaiRange(7, 10) +ADV_BUFFER_TIME = 5 + +LEAD_DIST = globalParameters.ADV_DIST + 10 +LEAD_SPEED = globalParameters.EGO_SPEED - 4 + +BYPASS_DIST = [15, 10] +SAFE_DIST = 15 +INIT_DIST = 50 +TERM_DIST = 70 +TERM_TIME = 10 + +################################# +# AGENT BEHAVIORS # +################################# + +behavior DecelerateBehavior(brake): + take SetBrakeAction(brake) + +behavior EgoBehavior(): + try: + do FollowLaneBehavior(target_speed=globalParameters.EGO_SPEED) + interrupt when (distance to adversary) < BYPASS_DIST[0]: + fasterLaneSec = self.laneSection.fasterLane + do LaneChangeBehavior( + laneSectionToSwitch=fasterLaneSec, + target_speed=globalParameters.EGO_SPEED) + try: + do FollowLaneBehavior( + target_speed=globalParameters.EGO_SPEED, + laneToFollow=fasterLaneSec.lane) \ + until (distance to adversary) > BYPASS_DIST[1] + interrupt when (distance to lead) < SAFE_DIST: + try: + do DecelerateBehavior(globalParameters.EGO_BRAKE) + interrupt when (distance to lead) > SAFE_DIST: + do FollowLaneBehavior(target_speed=LEAD_SPEED) for TERM_TIME seconds + terminate + +behavior AdversaryBehavior(): + do FollowLaneBehavior(target_speed=globalParameters.ADV_INIT_SPEED) \ + until self.lane is not ego.lane + do FollowLaneBehavior(target_speed=globalParameters.ADV_END_SPEED) + +behavior LeadBehavior(): + fasterLaneSec = self.laneSection.fasterLane + do LaneChangeBehavior( + laneSectionToSwitch=fasterLaneSec, + target_speed=LEAD_SPEED) + do FollowLaneBehavior(target_speed=LEAD_SPEED) + +################################# +# SPATIAL RELATIONS # +################################# + +initLane = Uniform(*network.lanes) +egoSpawnPt = new OrientedPoint in initLane.centerline + +################################# +# SCENARIO SPECIFICATION # +################################# + +ego = new Car at egoSpawnPt, + with blueprint MODEL, + with behavior EgoBehavior() + +adversary = new Car following roadDirection for globalParameters.ADV_DIST, + with blueprint MODEL, + with behavior AdversaryBehavior() + +lead = new Car following roadDirection for LEAD_DIST, + with blueprint MODEL, + with behavior LeadBehavior() + +require (distance to intersection) > INIT_DIST +require (distance from adversary to intersection) > INIT_DIST +require (distance from lead to intersection) > INIT_DIST +require always (adversary.laneSection._fasterLane is not None) +terminate when (distance to egoSpawnPt) > TERM_DIST diff --git a/tools/benchmarking/parallelization/benchmarks/city_intersection.scenic b/tools/benchmarking/parallelization/benchmarks/city_intersection.scenic new file mode 100644 index 000000000..6afe51efc --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/city_intersection.scenic @@ -0,0 +1,131 @@ +""" +Generate a city intersection driving scenario, an intersection +of two 2-lane one way roads in a city. +""" + +model scenic.simulators.webots.model + +import shapely +import time +import shutil +import os +from pathlib import Path + +class EgoCar(WebotsObject): + webotsName: "EGO" + shape: MeshShape.fromFile(globalParameters.meshBasePath / "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) + viewAngles: (1.5, 60 deg) + visibleDistance: 100 + rayDensity: 10 + +class Car(EgoCar): + webotsName: "CAR" + +class CommercialBuilding(WebotsObject): + webotsType: "BUILDING_COMMERCIAL" + width: 22 + length: 22 + height: 100 + yaw: Uniform(1, 2, 3) * 90 deg + +class ResidentialBuilding(WebotsObject): + webotsType: "BUILDING_RESIDENTIAL" + width: 14.275 + length: 57.4 + height: 40 + yaw: 90 deg + +class GlassBuilding(WebotsObject): + webotsType: "BUILDING_GLASS" + width: 14.1 + length: 8.1 + height: 112 + yaw: Uniform(1, 2, 3) * 90 deg + +class LogImageAction(Action): + def __init__(self, visible: bool, path: str, count: int): + self.visible = visible + self.path = path + self.count = count + + def applyTo(self, obj, sim): + print("Other Car Visible:", self.visible) + + target_path = self.path + "/" + target_path += "visible" if self.visible else "invisible" + + if not os.path.exists(target_path): + os.makedirs(target_path) + + target_path += "/" + str(self.count) + ".jpeg" + + print("IMG Path:", target_path) + + # Wait for other controller to write image + time.sleep(0.001) + attempts = 0 + while not os.path.exists(localPath("images/live_img.jpeg")): + print("Waiting for image...") + attempts += 1 + time.sleep(0.001) + + if attempts > 10: + print("Could not move image...") + return + + shutil.move(localPath("images/live_img.jpeg"), target_path) + +behavior LogCamera(path): + count = 0 + while True: + visible = ego can see car + take LogImageAction(visible, path, count) + count += 1 + +# Create a region that represents both lanes of the crossing road. +crossing_road_lane = RectangularRegion((0,0,0.02), 0, 160, 5) + +car = new Car facing 90 deg, on crossing_road_lane, with regionContainedIn crossing_road_lane +require car.x > 10 + +# Create a region that represents both lanes of the bottom road. +bottom_road_lane = RectangularRegion((0,-55,0.02), 0, 5, 80) + +# Place the ego car in one of the lanes, and ensure it is fully contained. +ego = new EgoCar on bottom_road_lane, with regionContainedIn bottom_road_lane, with behavior LogCamera(localPath(f"images/{time.time_ns()}")) + +# Create a region composed of all 4 quadrants around the road +top_right_quadrant = RectangularRegion(56@56, 0, 100, 100) +top_left_quadrant = RectangularRegion(-56@56, 0, 100, 100) +bottom_right_quadrant = RectangularRegion(56@-56, 0, 100, 100) +bottom_left_quadrant = RectangularRegion(-56@-56, 0, 100, 100) + +building_region = top_right_quadrant.union(top_left_quadrant) + +# Add buildings, some randomly, some designed to block visibility of the center road +for _ in range(1): + new CommercialBuilding in building_region, with regionContainedIn building_region + +for _ in range(2): + new ResidentialBuilding in building_region, with regionContainedIn building_region + +for _ in range(2): + new GlassBuilding in building_region, with regionContainedIn building_region + +new ResidentialBuilding at (-36, -21, 0) +new CommercialBuilding at (18 + Range(-1,1), -20 + Range(-1,1), 0), facing Range(-5,5) deg +new CommercialBuilding at (50 + Range(-1,1), -22 + Range(-1,1), 0), facing Range(-5,5) deg + +# Terminate the simulation after the ego has passed through the intersection or a timeout is reached +terminate when ego.position.y > 0 +terminate after 60 seconds + +# Require that the ego can eventually see the crossing car, but not until it gets close. +require eventually (ego can see car) +require (not ego can see car) until (distance from ego to car < 75) + +# Require that the cars do not crash +require always distance to car > 2 diff --git a/tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic b/tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic new file mode 100644 index 000000000..0856487cf --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/enclosed_occluded.scenic @@ -0,0 +1,20 @@ +""" Tests visibility calculation time on an object completely enclosed in two spheres, +trying to view the outside sphere. +""" + +from scipy.spatial.transform import Rotation +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object facing Orientation(Rotation.random()), at (Range(-0.1,0.1),Range(-0.1,0.1),Range(-0.1,0.1)) + +def get_shape(hole_size): + return MeshShape( + SpheroidRegion(dimensions=(1,1,1)).difference( + SpheroidRegion(dimensions=(0.9,0.9,0.9))).difference( + BoxRegion(dimensions=(hole_size,0.5,hole_size), position=(0,0.5,0))).mesh + ) + +occluding_sphere = new Object at (0,0,0), with shape get_shape(0.1), with width 3, with length 3, with height 3 +target_sphere = new Object at (0,0,0), with shape get_shape(0.3), with width 5, with length 5, with height 5, not visible diff --git a/tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic b/tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic new file mode 100644 index 000000000..52cc2b4d9 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/enclosed_visible.scenic @@ -0,0 +1,22 @@ +""" Tests visibility calculation time on an object almost completely enclosed in two spheres, +trying to view the outside sphere. +""" + +from scipy.spatial.transform import Rotation +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object facing Orientation(Rotation.random()), at (Range(-0.1,0.1),Range(-0.1,0.1),Range(-0.1,0.1)) + +hollow_sphere_shape = MeshShape( + SpheroidRegion(dimensions=(5,5,5)).difference(SpheroidRegion(dimensions=(4.8,4.8,4.8))).mesh + ) + +hollow_sphere_shape_with_hole = MeshShape( + SpheroidRegion(dimensions=(5.2,5.2,5.2)).difference(SpheroidRegion(dimensions=(5.01,5.01,5.01))).difference( + BoxRegion(dimensions=(0.1,0.1,1), position=(0,0,2.5))).mesh + ) + +occluding_sphere = new Object at (0,0,0), with shape hollow_sphere_shape_with_hole +target_sphere = new Object at (0,0,0), with shape hollow_sphere_shape, visible diff --git a/tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic b/tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic new file mode 100644 index 000000000..b61ade734 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/fully_occluded.scenic @@ -0,0 +1,15 @@ +""" Tests visibility calculation time on a complex mesh, completely occluded by another complex mesh""" + +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object + +chair_shape = MeshShape.fromFile(path=globalParameters.meshBasePath / "chair.obj.bz2", initial_rotation=(0,90 deg,0)) + +obscuring_chair = new Object with shape chair_shape, at (0,5,0), + with pitch -90 deg, with width 5, with length 5, with height 5 + +target_chair = new Object with shape chair_shape, at (0,10,0), + with width 3, with length 3, with height 3, not visible diff --git a/tools/benchmarking/parallelization/benchmarks/fully_visible.scenic b/tools/benchmarking/parallelization/benchmarks/fully_visible.scenic new file mode 100644 index 000000000..59d2935f9 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/fully_visible.scenic @@ -0,0 +1,12 @@ +""" Tests visibility calculation time on a complex mesh that doesn't contain its center """ + +from pathlib import Path + +workspace = Workspace(everywhere) + +ego = new Object + +chair_shape = MeshShape.fromFile(path=globalParameters.meshBasePath / "chair.obj.bz2", initial_rotation=(0,90 deg,0)) + +target_chair = new Object with shape chair_shape, at (0,10,0), + with width 3, with length 3, with height 3, visible diff --git a/tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic b/tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic new file mode 100644 index 000000000..2199d91e0 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/narrowGoalNew.scenic @@ -0,0 +1,102 @@ +model scenic.simulators.webots.model + +from pathlib import Path + +# Set up workspace +width = 10 +length = 10 +workspace = Workspace(RectangularRegion(0 @ 0, 0, width, length)) + +# types of objects + +class MarsGround(Ground): + width: width + length: length + gridSize: 20 + +class MarsHill(Hill): + position: new Point in workspace + width: Range(1,2) + length: Range(1,2) + height: Range(0.1, 0.3) + spread: Range(0.2, 0.3) + regionContainedIn: everywhere + +class Goal(WebotsObject): + """Flag indicating the goal location.""" + width: 0.1 + length: 0.1 + webotsType: 'GOAL' + +class Rover(WebotsObject): + """Mars rover.""" + width: 0.5 + length: 0.7 + height: 0.4 + webotsType: 'ROVER' + rotationOffset: (90 deg, 0, 0) + +class Debris(WebotsObject): + """Abstract class for debris scattered randomly in the workspace.""" + # Recess things into the ground slightly by default + baseOffset: (0, 0, -self.height/3) + +class BigRock(Debris): + """Large rock.""" + shape: MeshShape.fromFile(globalParameters.meshBasePath / "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(globalParameters.meshBasePath / "webots_rock_small.obj.bz2") + yaw: Range(0, 360 deg) + webotsType: 'ROCK_SMALL' + positionOffset: Vector(0,0, -self.height/2) + +class Pipe(Debris): + """Pipe with variable length.""" + width: 0.2 + length: Range(0.5, 1.5) + height: self.width + shape: CylinderShape(initial_rotation=(90 deg, 0, 90 deg)) + yaw: Range(0, 360 deg) + webotsType: 'PIPE' + rotationOffset: (90 deg, 0, 90 deg) + + def startDynamicSimulation(self): + # Apply variable length + self.webotsObject.getField('height').setSFFloat(self.length) + +# Ground with random gaussian hills +ground = new MarsGround on (0,0,0), with terrain [new MarsHill for _ in range(60)] + +# Ego and goal on ground +ego = new Rover at (0, -3), on ground, with controller 'sojourner' +goal = new Goal at (Range(-2, 2), Range(2, 3)), on ground, facing (0,0,0) + +# Bottleneck made of two pipes with a rock in between +bottleneck = new OrientedPoint at ego offset by Range(-1.5, 1.5) @ Range(0.5, 1.5), facing Range(-30, 30) deg +require abs((angle to goal) - (angle to bottleneck)) <= 10 deg +new BigRock at bottleneck, on ground + +gap = 1.2 * ego.width +halfGap = gap / 2 + +leftEdge = new OrientedPoint left of bottleneck by halfGap, + facing Range(60, 120) deg relative to bottleneck.heading +rightEdge = new OrientedPoint right of bottleneck by halfGap, + facing Range(-120, -60) deg relative to bottleneck.heading + +new Pipe ahead of leftEdge, with length Range(1, 2), on ground, facing leftEdge, with parentOrientation 0 +new Pipe ahead of rightEdge, with length Range(1, 2), on ground, facing rightEdge, with parentOrientation 0 + +# Other junk because why not? + +new Pipe on ground, with parentOrientation 0 +new BigRock beyond bottleneck by Range(0.25, 0.75) @ Range(0.75, 1), on ground +new BigRock beyond bottleneck by Range(-0.75, -0.25) @ Range(0.75, 1), on ground +new Rock on ground +new Rock on ground +new Rock on ground diff --git a/tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic b/tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic new file mode 100644 index 000000000..aec4524bd --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/narrowGoalOld.scenic @@ -0,0 +1,83 @@ +from scenic.simulators.webots.model import WebotsObject + +# Set up workspace +width = 5 +length = 5 +workspace = Workspace(RectangularRegion(0 @ 0, 0, width, length)) + +# types of objects + +class Goal(WebotsObject): + """Flag indicating the goal location.""" + width: 0.3 + length: 0.3 + webotsType: 'GOAL' + +class Rover(WebotsObject): + """Mars rover.""" + width: 0.5 + length: 0.7 + webotsType: 'ROVER' + rotationOffset: 90 deg + +class Debris(WebotsObject): + """Abstract class for debris scattered randomly in the workspace.""" + position: new Point in workspace + heading: Range(0, 360) deg + +class BigRock(Debris): + """Large rock.""" + width: 0.17 + length: 0.17 + webotsType: 'ROCK_BIG' + +class Rock(Debris): + """Small rock.""" + width: 0.10 + length: 0.10 + webotsType: 'ROCK_SMALL' + +class Pipe(Debris): + """Pipe with variable length.""" + width: 0.2 + length: Range(0.5, 1.5) + webotsType: 'PIPE' + + def startDynamicSimulation(self): + # Apply variable length + self.webotsObject.getField('height').setSFFloat(self.length) + # Apply 3D rotation to make pipes lie flat on surface + rotation = [cos(self.heading), sin(self.heading), 0, 90 deg] + self.webotsObject.getField('rotation').setSFRotation(rotation) + +ego = new Rover at 0 @ -2 + +goal = new Goal at Range(-2, 2) @ Range(2, 2.5) + +# Bottleneck made of two pipes with a rock in between + +gap = 1.2 * ego.width +halfGap = gap / 2 + +bottleneck = new OrientedPoint offset by Range(-1.5, 1.5) @ Range(0.5, 1.5), facing Range(-30, 30) deg + +require abs((angle to goal) - (angle to bottleneck)) <= 10 deg + +new BigRock at bottleneck + +leftEdge = new OrientedPoint at bottleneck offset by -halfGap @ 0, + facing Range(60, 120) deg relative to bottleneck.heading +rightEdge = new OrientedPoint at bottleneck offset by halfGap @ 0, + facing Range(-120, -60) deg relative to bottleneck.heading + +new Pipe ahead of leftEdge, with length Range(1, 2) +new Pipe ahead of rightEdge, with length Range(1, 2) + +# Other junk because why not? + +new Pipe +new BigRock beyond bottleneck by Range(-0.5, 0.5) @ Range(0.5, 1) +new BigRock beyond bottleneck by Range(-0.5, 0.5) @ Range(0.5, 1) +new Rock +new Rock +new Rock \ No newline at end of file diff --git a/tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic b/tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic new file mode 100644 index 000000000..5d2fc408e --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/pedestrian_02.scenic @@ -0,0 +1,89 @@ +""" +TITLE: Pedestrian 02 +AUTHOR: Francis Indaheng, findaheng@berkeley.edu +DESCRIPTION: Both ego and adversary vehicles must suddenly stop to avoid +collision when pedestrian crosses the road unexpectedly. +SOURCE: Carla Challenge, #03 +""" + +################################# +# MAP AND MODEL # +################################# + +model scenic.simulators.carla.model + +################################# +# CONSTANTS # +################################# + +MODEL = 'vehicle.lincoln.mkz2017' + +param EGO_INIT_DIST = VerifaiRange(-30, -20) +param EGO_SPEED = VerifaiRange(7, 10) +EGO_BRAKE = 1.0 + +param ADV_INIT_DIST = VerifaiRange(40, 50) +param ADV_SPEED = VerifaiRange(7, 10) +ADV_BRAKE = 1.0 + +PED_MIN_SPEED = 1.0 +PED_THRESHOLD = 20 + +param SAFETY_DIST = VerifaiRange(10, 15) +BUFFER_DIST = 75 +CRASH_DIST = 5 +TERM_DIST = 50 + +################################# +# AGENT BEHAVIORS # +################################# + +behavior EgoBehavior(): + try: + do FollowLaneBehavior(target_speed=globalParameters.EGO_SPEED) + interrupt when withinDistanceToObjsInLane(self, globalParameters.SAFETY_DIST) and (ped in network.drivableRegion): + take SetBrakeAction(EGO_BRAKE) + interrupt when withinDistanceToAnyObjs(self, CRASH_DIST): + terminate + +behavior AdvBehavior(): + try: + do FollowLaneBehavior(target_speed=globalParameters.ADV_SPEED) + interrupt when (withinDistanceToObjsInLane(self, globalParameters.SAFETY_DIST) or (distance from adv to ped) < 10) and (ped in network.drivableRegion): + take SetBrakeAction(ADV_BRAKE) + interrupt when withinDistanceToAnyObjs(self, CRASH_DIST): + terminate + +################################# +# SPATIAL RELATIONS # +################################# + +road = Uniform(*filter(lambda r: len(r.forwardLanes.lanes) == len(r.backwardLanes.lanes) == 1, network.roads)) +egoLane = Uniform(road.forwardLanes.lanes)[0] +spawnPt = new OrientedPoint on egoLane.centerline +advSpawnPt = new OrientedPoint following roadDirection from spawnPt for globalParameters.ADV_INIT_DIST + +################################# +# SCENARIO SPECIFICATION # +################################# + +ego = new Car following roadDirection from spawnPt for globalParameters.EGO_INIT_DIST, + with blueprint MODEL, + with behavior EgoBehavior() + +ped = new Pedestrian right of spawnPt by 3, + with heading 90 deg relative to spawnPt.heading, + with regionContainedIn None, + with behavior CrossingBehavior(ego, PED_MIN_SPEED, PED_THRESHOLD) + +adv = new Car left of advSpawnPt by 3, + with blueprint MODEL, + with heading 180 deg relative to spawnPt.heading, + with behavior AdvBehavior() + +require (distance from spawnPt to intersection) > BUFFER_DIST +require always (ego.laneSection._slowerLane is None) +require always (ego.laneSection._fasterLane is None) +require always (adv.laneSection._slowerLane is None) +require always (adv.laneSection._fasterLane is None) +terminate when (distance to spawnPt) > TERM_DIST diff --git a/tools/benchmarking/parallelization/benchmarks/vacuum.scenic b/tools/benchmarking/parallelization/benchmarks/vacuum.scenic new file mode 100644 index 000000000..0b11fe6db --- /dev/null +++ b/tools/benchmarking/parallelization/benchmarks/vacuum.scenic @@ -0,0 +1,149 @@ +""" +Generate a room for the i-roomba create vacuum +""" +model scenic.simulators.webots.model + +import numpy as np +import trimesh +import random +from pathlib import Path + +param numToys = 0 +param duration = 10 + +## Class Definitions ## + +class Vacuum(WebotsObject): + webotsName: "IROBOT_CREATE" + shape: CylinderShape() + width: 0.335 + length: 0.335 + height: 0.07 + customData: str(random.getrandbits(32)) # Random seed for robot controller + +# Floor uses builtin Webots floor to keep Vacuum Sensors from breaking +# Not actually linked to WebotsObject because Webots floor is 2D +class Floor(Object): + width: 5 + length: 5 + height: 0.01 + position: (0,0,-0.005) + +class Wall(WebotsObject): + webotsAdhoc: {'physics': False} + width: 5 + length: 0.04 + height: 0.5 + +class DiningTable(WebotsObject): + webotsAdhoc: {'physics': True} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "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 + +class DiningChair(WebotsObject): + webotsAdhoc: {'physics': True} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "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) + +class Couch(WebotsObject): + webotsAdhoc: {'physics': False} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "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) + +class CoffeeTable(WebotsObject): + webotsAdhoc: {'physics': False} + shape: MeshShape.fromFile(globalParameters.meshBasePath / "coffee_table.obj.bz2") + width: 1.5 + length: 0.5 + height: 0.4 + positionStdDev: (0.05, 0.05 ,0) + orientationStdDev: (5 deg, 0, 0) + +class Toy(WebotsObject): + webotsAdhoc: {'physics': True} + shape: Uniform(BoxShape(), CylinderShape(), ConeShape(), SpheroidShape()) + width: 0.1 + length: 0.1 + height: 0.1 + density: 100 + +class BlockToy(Toy): + shape: BoxShape() + +## Scene Layout ## + +# Create room region and set it as the workspace +room_region = RectangularRegion(0 @ 0, 0, 5.09, 5.09) +workspace = Workspace(room_region) + +# Create floor and walls +floor = new Floor +wall_offset = floor.width/2 + 0.04/2 + 1e-4 +right_wall = new Wall at (wall_offset, 0, 0.25), facing toward floor +left_wall = new Wall at (-wall_offset, 0, 0.25), facing toward floor +front_wall = new Wall at (0, wall_offset, 0.25), facing toward floor +back_wall = new Wall at (0, -wall_offset, 0.25), facing toward floor + +# Place vacuum on floor +ego = new Vacuum on floor + +# Create a "safe zone" around the vacuum so that it does not start stuck +safe_zone = CircularRegion(ego.position, radius=1) + +# Create a dining room region where we will place dining room furniture +dining_room_region = RectangularRegion(1.25 @ 0, 0, 2.5, 5).difference(safe_zone) + +# Place a table with 3 chairs around it, and one knocked over on the floor +dining_table = new DiningTable contained in dining_room_region, on floor, + facing Range(0, 360 deg) + +chair_1 = new DiningChair behind dining_table by -0.1, on floor, + facing toward dining_table, with regionContainedIn dining_room_region +chair_2 = new DiningChair ahead of dining_table by -0.1, on floor, + facing toward dining_table, with regionContainedIn dining_room_region +chair_3 = new DiningChair left of dining_table by -0.1, on floor, + facing toward dining_table, with regionContainedIn dining_room_region + +fallen_orientation = Uniform((0, -90 deg, 0), (0, 90 deg, 0), (0, 0, -90 deg), (0, 0, 90 deg)) + +chair_4 = new DiningChair contained in dining_room_region, facing fallen_orientation, + on floor, with baseOffset(0,0,-0.2) + +# Add some noise to the positions and yaw of the chairs around the table +mutate chair_1, chair_2, chair_3 + +# Create a living room region where we will place living room furniture +living_room_region = RectangularRegion(-1.25 @ 0, 0, 2.5, 5).difference(safe_zone) + +couch = new Couch ahead of left_wall by 0.335, + on floor, facing away from left_wall + +coffee_table = new CoffeeTable ahead of couch by 0.336, + on floor, facing away from couch + +# Add some noise to the positions of the couch and coffee table +mutate couch, coffee_table + +toy_stack = new BlockToy on floor +toy_stack = new BlockToy on toy_stack +toy_stack = new BlockToy on toy_stack + +# Spawn some toys +for _ in range(globalParameters.numToys): + new Toy on floor + +## Simulation Setup ## +terminate after globalParameters.duration * 60 seconds +record (ego.x, ego.y) as VacuumPosition From 6ff049afa9eca8eb99baf3d4af52564ba7e9a872 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 25 Nov 2025 13:11:49 -0800 Subject: [PATCH 008/134] Fixed coordinate system --- src/scenic/core/serialization.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 006fdf925..731589fcb 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -405,10 +405,9 @@ def toOpenScenario( continue veh_name = obj.name if hasattr(obj, "name") else f"Vehicle{obj_i}" - # NOTE: XOSC coordinate system swaps X and Y compared to Scenic. veh_bb = xosc.BoundingBox( - obj.length, obj.width, + obj.length, obj.height, 0, 0, @@ -446,7 +445,9 @@ def toOpenScenario( init = xosc.Init() for obj, xosc_obj in xosc_objects.items(): - init_position = xosc.WorldPosition(x=obj.x, y=obj.y, z=obj.z, h=obj.heading) + init_position = xosc.WorldPosition( + x=obj.x, y=obj.y, z=obj.z, h=obj.heading + math.radians(90) + ) obj_init_action = xosc.TeleportAction(init_position) init.add_init_action(xosc_obj.name, obj_init_action) @@ -466,7 +467,7 @@ def toOpenScenario( action_positions = [] for t, states in enumerate(simulationResult.trajectory): state_position = states.positions[obj_i] - state_orientation = states.orientations[obj_i].yaw + state_orientation = states.orientations[obj_i].yaw + math.radians(90) action_times.append(simulationResult.timestep * t) pos = xosc.WorldPosition( x=state_position.x, From 0b45e3d4ade8c101b8f26bb90115bb25f4777615 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 25 Nov 2025 13:43:39 -0800 Subject: [PATCH 009/134] Updated dependencies and fixed wheelbase offset --- pyproject.toml | 4 ++++ src/scenic/core/serialization.py | 20 ++++++++++++++++---- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index b1ade058c..cc988206a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -59,6 +59,9 @@ metadrive = [ "metadrive-simulator@git+https://github.com/metadriverse/metadrive.git@85e5dadc6c7436d324348f6e3d8f8e680c06b4db", "sumolib >= 1.21.0", ] +openscenario = [ + "scenariogeneration" +] test = [ # minimum dependencies for running tests (used for tox virtualenvs) "pytest >= 7.0.0, <9", "pytest-cov >= 3.0.0", @@ -68,6 +71,7 @@ test-full = [ # like 'test' but adds dependencies for optional features "scenic[test]", # all dependencies from 'test' extra above "scenic[guideways]", # for running guideways modules "scenic[metadrive]", + "scenic[openscenario]", "astor >= 0.8.1", 'carla >= 0.9.12; python_version <= "3.10" and (platform_system == "Linux" or platform_system == "Windows")', "dill", diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 731589fcb..b4a66d516 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -15,6 +15,7 @@ from scenic.core.distributions import Samplable, needsSampling from scenic.core.utils import DefaultIdentityDict +from scenic.core.vectors import Vector ## JSON @@ -411,7 +412,7 @@ def toOpenScenario( obj.height, 0, 0, - obj.height / 2, + 0, ) veh_fa = xosc.Axle( maxSteeringAngle, @@ -445,8 +446,16 @@ def toOpenScenario( init = xosc.Init() for obj, xosc_obj in xosc_objects.items(): + scenic_yaw = obj.yaw + state_orientation = scenic_yaw + math.radians(90) + state_position = obj.position.offsetRotated( + scenic_yaw, Vector(0, -0.5 * wheelbaseRatio * obj.length, 0) + ) init_position = xosc.WorldPosition( - x=obj.x, y=obj.y, z=obj.z, h=obj.heading + math.radians(90) + x=state_position.x, + y=state_position.y, + z=state_position.z, + h=state_orientation, ) obj_init_action = xosc.TeleportAction(init_position) init.add_init_action(xosc_obj.name, obj_init_action) @@ -466,8 +475,11 @@ def toOpenScenario( action_times = [] action_positions = [] for t, states in enumerate(simulationResult.trajectory): - state_position = states.positions[obj_i] - state_orientation = states.orientations[obj_i].yaw + math.radians(90) + scenic_yaw = states.orientations[obj_i].yaw + state_orientation = scenic_yaw + math.radians(90) + state_position = states.positions[obj_i].offsetRotated( + scenic_yaw, Vector(0, -0.5 * wheelbaseRatio * obj.length, 0) + ) action_times.append(simulationResult.timestep * t) pos = xosc.WorldPosition( x=state_position.x, From cf3c6479f530ebcd2dba37e9fe1508424b24fe60 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 1 Dec 2025 20:04:33 -0800 Subject: [PATCH 010/134] Simulation parallelization --- src/scenic/core/scenarios.py | 145 ++++++++--- src/scenic/core/simulators.py | 236 +++++++++++++++++- src/scenic/core/utils.py | 33 +-- tests/core/test_scenarios.py | 3 + ...ization.py => benchmark_scene_parallel.py} | 38 +-- .../parallelization/benchmark_sim_parallel.py | 89 +++++++ .../benchmarks/badlyParkedCarPullingIn.scenic | 2 +- .../benchmarks/bypassing_03.scenic | 3 +- 8 files changed, 458 insertions(+), 91 deletions(-) rename tools/benchmarking/parallelization/{benchmark_parallelization.py => benchmark_scene_parallel.py} (73%) create mode 100644 tools/benchmarking/parallelization/benchmark_sim_parallel.py diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index 362aaa715..852680ada 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -4,6 +4,7 @@ import io import itertools import multiprocessing +import os import random import sys import time @@ -40,7 +41,7 @@ ) from scenic.core.sample_checking import BasicChecker, WeightedAcceptanceChecker from scenic.core.serialization import Serializer, dumpAsScenicCode -from scenic.core.utils import generateInnerBatchHelper +from scenic.core.utils import setSeed from scenic.core.vectors import Vector # Global params @@ -423,7 +424,6 @@ def generateBatch( feedback=None, numWorkers=0, mute=True, - deterministic=True, serialized=False, ): """Sample several `Scene` objects from this scenario. @@ -439,7 +439,6 @@ def generateBatch( numWorkers (int): The number of workers to be used when generating scenes. If numWorkers is 0, scenes will be generated in the main process. mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. - deterministic (bool): Whether or not scenes will be returned in a deterministic order. serialized (bool): Whether or not to return scenes in a serialized format. Returns: @@ -456,8 +455,9 @@ def generateBatch( feedback=feedback, numWorkers=numWorkers, mute=mute, - deterministic=deterministic, serialized=serialized, + deterministic=True, + iterationCount=True, ) results_list = list(stream) scenes = tuple(r[0] for r in results_list) @@ -471,38 +471,69 @@ def generateStream( verbosity=0, feedback=None, numWorkers=0, + bufferSize=None, mute=True, - deterministic=False, serialized=False, + deterministic=True, + iterationCount=False, ): - """Sample several `Scene` objects from this scenario. + """Sample a stream of `Scene` objects from this scenario. - For a description of how scene generation is done, see `scene generation`. + For a description of how scene generation is done, see `scene generation`. This function can produce + both finite and infinite streams (depending on the value of `numScenes`). + + .. note:: + NOTE: The deterministic parameter is by default set to True, meaning that scenes + will be returned in a fixed order for a given Scenic seed. Setting this to False + means scenes will be returned in a possibly non-deterministic order, but with possibly + decreased latency. When deterministic is set to False, the ordering of the returned scenes + is not fully random, with scenes that are easier to generate being more likely to be returned + earlier in the stream. Despite this, the overall distribution of the returned scenes still + matches the scenario. When generating an infinite stream, `deterministic` must be set to True. Args: - numScenes (int): Number of scenes to generate. + numScenes (int): Number of scenes to generate, or `float('inf')` to sample an infinite stream + of Scenes. maxIterations (int): Maximum number of rejection sampling iterations (over all scenes). verbosity (int): Verbosity level. feedback (float): Feedback to pass to external samplers doing active sampling. See :mod:`scenic.core.external_params`. numWorkers (int): The number of workers to be used when generating scenes. If numWorkers is 0, scenes will be generated in the main process. + bufferSize (int): The number of scenes to have available at any given time, or `None` to + use default values. If set to `None` and `numScenes` is finite, all scenes are generated + in a greedy fashon. If set to `None and `numScenes` is infinite, set to a default + value of `2*numWorkers`. mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. - deterministic (bool): Whether or not scenes will be returned in a deterministic order. - NOTE: Setting this to True may increase latency when waiting for the next `Scene` in - the stream. serialized (bool): Whether or not to return scenes in a serialized format. + deterministic (bool): Whether or not scenes will be returned in a deterministic order. This + must be set to `True` when generating an infinite stream. + iterationCount (bool): Whether or not to return the number of iterations used to generate each + scene. If this is set to `False`, the return type is simply an iterable of `Scene` objects. Returns: - An iterable of pairs with a sampled `Scene` and the number of iterations used for that scene. - - Raises: - `RejectionException`: if not enough valid samples are found in **maxIterations** iterations. + An iterable of pairs with a sampled `Scene` and the number of iterations used for that scene + (if iterationCount is set to True). """ + if numScenes <= 0: + raise ValueError("`numScenes` must be at least 1.") + + if not isinstance(numScenes, int) and numScenes != float("inf"): + raise ValueError("`numScenes` must be either an `int` or `float('inf')`.") + + if numScenes == float("inf") and not deterministic: + raise ValueError( + "`deterministic` must be set to `True` when generating an infinite stream." + ) + + if bufferSize is None and numScenes == float("inf"): + bufferSize = 2 * numWorkers + if numWorkers == 0: totalIterations = 0 + returnedResultCount = 0 - for _ in range(numScenes): + while returnedResultCount < numScenes: try: remainingIts = maxIterations - totalIterations scene, iterations = self._generateInner( @@ -510,33 +541,39 @@ def generateStream( ) totalIterations += iterations yield (scene, iterations) + returnedResultCount += 1 except RejectionException: raise RejectionException( f"failed to generate scenario in {maxIterations} iterations" ) else: if maxIterations != float("inf"): - raise RuntimeError("maxIterations not supported for parallel sampling.") + raise ValueError("maxIterations not supported for parallel sampling.") if feedback is not None: - raise RuntimeError("Feedback not supported for parallel sampling.") + raise ValueError("Feedback not supported for parallel sampling.") # Initialize results tracking data - resultsList = [] - returnedResults = 0 + returnedResultCount = 0 + + # Initialize random generator + rand_generator = numpy.random.default_rng(random.getrandbits(32)) - # Initialize queues and lock + # Initialize queues seedQueue = multiprocessing.Queue() - seedHistory = {} + seedHistory = [] + putSeedCount = 0 def putSeed(): - newSeed = random.getrandbits(32) + nonlocal putSeedCount + newSeed = int(rand_generator.integers(2**32)) seedQueue.put(newSeed) - seedHistory[newSeed] = len(seedHistory) if deterministic: - resultsList.append(None) + seedHistory.append(newSeed) + putSeedCount += 1 - for _ in range(numScenes): + initialSeedCount = numScenes if bufferSize is None else bufferSize + for _ in range(initialSeedCount): putSeed() sceneQueue = multiprocessing.Queue() @@ -549,6 +586,8 @@ def putSeed(): ] # Initialized result management functions + resultsDict = {} + def getResult(): sceneBytes, resultIterations, resultSeed = sceneQueue.get() resultScene = ( @@ -560,28 +599,30 @@ def getResult(): def getNextResult(): if not deterministic: - return getResult()[0] + nextResult = getResult()[0] + return nextResult if iterationCount else nextResult[0] while True: - assert len(resultsList) > 0 - - if resultsList[0] is not None: - return resultsList.pop(0) + if seedHistory[0] in resultsDict: + nextResult = resultsDict[seedHistory.pop(0)] + return nextResult if iterationCount else nextResult[0] result, resultSeed = getResult() - resultIndex = seedHistory[resultSeed] - returnedResults - assert resultsList[resultIndex] is None - resultsList[resultIndex] = result + resultsDict[resultSeed] = result # Start sampling processes and yield samples try: - # Prepare process pool + # Start processes for process in processes: process.start() - for _ in range(numScenes): + # Retrieve results + while returnedResultCount < numScenes: yield getNextResult() - returnedResults += 1 + returnedResultCount += 1 + + if putSeedCount < numWorkers: + putSeed() finally: # Close processes and queues @@ -904,3 +945,33 @@ def simulationFromBytes( data = io.BytesIO(data) scene = self.sceneFromBytes(data, verify=verify, allowPickle=allowPickle) return simulator.simulate(scene, replay=data, **kwargs) + + +def generateInnerBatchHelper( + scenarioCreationData, seedQueue, sceneQueue, verbosity, mute +): + if mute: + sys.stdout = open(os.devnull, "w") + sys.stderr = open(os.devnull, "w") + + from scenic.syntax.translator import _scenarioFromStream + + scenario = _scenarioFromStream( + stream=io.BytesIO(scenarioCreationData["streamLines"]), + compileOptions=scenarioCreationData["compileOptions"], + filename=scenarioCreationData["filename"], + scenario=scenarioCreationData["scenario"], + path=scenarioCreationData["path"], + _cacheImports=False, + ) + + while True: + seed = seedQueue.get() + setSeed(seed) + + scene, iterations = scenario._generateInner( + maxIterations=float("inf"), verbosity=verbosity, feedback=None + ) + sceneBytes = scenario.sceneToBytes(scene) + + sceneQueue.put((sceneBytes, iterations, seed)) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 3e9c0308f..aa550ed1c 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -13,10 +13,18 @@ import abc from collections import defaultdict import enum +import io import math +import multiprocessing import numbers +import os +import random +import sys import time import types +import warnings + +import numpy from scenic.core.distributions import RejectionException from scenic.core.dynamics import GuardViolation, RejectSimulationException @@ -31,6 +39,7 @@ ) from scenic.core.requirements import RequirementType from scenic.core.serialization import Serializer +from scenic.core.utils import setSeed from scenic.core.vectors import Vector @@ -393,6 +402,7 @@ def __init__( # Package up simulation results into a compact object. result = SimulationResult( + self.name, self.trajectory, self.actionSequence, terminationType, @@ -922,6 +932,7 @@ class SimulationResult: """Result of running a simulation. Attributes: + name: Name of the simulation, if any trajectory: A tuple giving for each time step the simulation's 'state': by default the positions of every object. See `Simulation.currentState`. finalState: The last 'state' of the simulation, as above. @@ -934,7 +945,10 @@ class SimulationResult: values its expression took during the simulation. """ - def __init__(self, trajectory, actions, terminationType, terminationReason, records): + def __init__( + self, name, trajectory, actions, terminationType, terminationReason, records + ): + self.name = name self.trajectory = tuple(trajectory) assert self.trajectory self.finalState = self.trajectory[-1] @@ -942,3 +956,223 @@ def __init__(self, trajectory, actions, terminationType, terminationReason, reco self.terminationType = terminationType self.terminationReason = str(terminationReason) self.records = dict(records) + + +class SimulatorGroup: + def __init__( + self, + numWorkers, + simulatorClass, + simulatorParams=None, + bufferSize=None, + mute=True, + returnFinalState=False, + returnTrajectory=False, + ): + if numWorkers <= 0: + raise ValueError("`numWorkers` must be at least 1.") + self.numWorkers = numWorkers + self.simulatorClass = simulatorClass + simulatorParams = simulatorParams if simulatorParams else dict() + if isinstance(simulatorParams, dict): + self.simulatorParams = [simulatorParams for _ in range(numWorkers)] + elif isinstance(simulatorParams, collections.abc.Iterable): + if len(simulatorParams) != numWorkers: + raise ValueError( + "Length of `simulatorParams` does not match `numWorkers`." + ) + self.simulatorParams = tuple(simulatorParams) + else: + raise ValueError("`simulatorParams` is not a dict or iterable of dicts.") + self.bufferSize = 2 * numWorkers if bufferSize is None else bufferSize + if self.bufferSize <= 1: + raise ValueError("`bufferSize` must be at least 1.") + self.mute = mute + self.returnFinalState = returnFinalState + self.returnTrajectory = returnTrajectory + + def _jobName(self, jobId): + return f"Scene{jobId}" + + def _serializeScene(self, scene, scenario, serialized): + from scenic.core.scenarios import Scene + + if serialized: + if not isinstance(scene, bytes): + raise ValueError( + f"Scene provided has type `{type(scene)}` instead of type `bytes`, but serialized was set to True." + ) + return scene + else: + if not isinstance(scene, Scene): + raise ValueError( + f"Scene provided has type `{type(scene)}` instead of type `Scene`, but serialized was set to False." + ) + return scenario.sceneFromBytes(scene, verify=True) + + def _prepareJob( + self, scene, simulateParams, jobId, scenario, serialized, rand_generator + ): + serializedScene = self._serializeScene(scene, scenario, serialized) + jobParams = simulateParams.copy() + + if "name" in jobParams: + warnings.warn( + "`name` in `simulateParams` is ignored and overwritten by custom name when using `SimulatorGroup`." + ) + jobName = self._jobName(jobId) + jobParams["name"] = jobName + + seed = int(rand_generator.integers(2**32)) + + return (jobId, serializedScene, jobParams, seed) + + def simulateBatch(self, scenario, scenes, simulateParams=None, serialized=True): + return tuple( + v[1] + for v in self.simulateStream( + scenario, scenes, simulateParams, serialized, deterministic=True + ) + ) + + def simulateStream( + self, scenario, scenes, simulateParams=None, serialized=True, deterministic=False + ): + simulateParams = simulateParams if simulateParams else dict() + + # Create helper parameters + scenarioCreationData = scenario._scenarioCreationData + jobQueue = multiprocessing.Queue() + resultQueue = multiprocessing.Queue() + + # Initialize random generator + rand_generator = numpy.random.default_rng(random.getrandbits(32)) + + # Initialize processes + processes = [] + for simulatorParams in self.simulatorParams: + params = ( + scenarioCreationData, + self.simulatorClass, + simulatorParams, + jobQueue, + resultQueue, + self.mute, + self.returnFinalState, + self.returnTrajectory, + ) + processes.append( + multiprocessing.Process(target=simulatorGroupHelper, args=params) + ) + + # Job creation utilities + remainingJobs = 0 + jobId = 0 + + def putJob(scene): + nonlocal jobId + nonlocal remainingJobs + preparedJob = self._prepareJob( + scene, + simulateParams, + jobId, + scenario, + serialized, + rand_generator=rand_generator, + ) + jobQueue.put(preparedJob) + jobId += 1 + remainingJobs += 1 + + # Initialized result management functions + lastReturnedJob = 0 + resultsDict = {} + + def getNextResult(): + if not deterministic: + return resultQueue.get() + + nonlocal lastReturnedJob + while True: + if lastReturnedJob in resultsDict: + returnResult = (lastReturnedJob, resultsDict.pop(lastReturnedJob)) + lastReturnedJob += 1 + return returnResult + + jobId, nextResult = resultQueue.get() + resultsDict[jobId] = nextResult + + try: + # Start processes + for process in processes: + process.start() + + # Initially saturate job buffer + for _ in range(self.bufferSize): + if scene := next(scenes, None): + putJob(scene) + + # Retrieve results and replenish buffer + while remainingJobs: + simulationResult = getNextResult() + remainingJobs -= 1 + + if scene := next(scenes, None): + putJob(scene) + + yield simulationResult + + finally: + # Close processes and queues + for process in processes: + process.terminate() + + jobQueue.close() + resultQueue.close() + + +def simulatorGroupHelper( + scenarioCreationData, + simulatorClass, + simulatorParams, + jobQueue, + resultQueue, + mute, + returnFinalState, + returnTrajectory, +): + if mute: + sys.stdout = open(os.devnull, "w") + sys.stderr = open(os.devnull, "w") + + from scenic.syntax.translator import _scenarioFromStream + + scenario = _scenarioFromStream( + stream=io.BytesIO(scenarioCreationData["streamLines"]), + compileOptions=scenarioCreationData["compileOptions"], + filename=scenarioCreationData["filename"], + scenario=scenarioCreationData["scenario"], + path=scenarioCreationData["path"], + _cacheImports=False, + ) + + simulator = simulatorClass(**simulatorParams) + + while True: + jobId, serializedScene, simulateParams, seed = jobQueue.get() + setSeed(seed) + + scene = scenario.sceneFromBytes(serializedScene, verify=False) + simulation = simulator.simulate(scene, **simulateParams) + + if simulation: + simulationResult = simulation.result + simulationResult.actions = None + if not returnFinalState: + simulationResult.finalState = None + if not returnTrajectory: + simulationResult.trajectory = None + else: + simulationResult = None + + resultQueue.put((jobId, simulationResult)) diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 632557641..5266af78d 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -7,6 +7,7 @@ import io import itertools import math +import multiprocessing import os import random import signal @@ -400,35 +401,3 @@ def get_type_hints(obj, globalns=None, localns=None): def setSeed(seed): random.seed(seed) numpy.random.seed(seed) - - -def generateInnerBatchHelper( - scenarioCreationData, seedQueue, sceneQueue, verbosity, mute -): - if mute: - sys.stdout = open(os.devnull, "w") - sys.stderr = open(os.devnull, "w") - - from scenic.syntax.translator import _scenarioFromStream - - stream = io.BytesIO(scenarioCreationData["streamLines"]) - - scenario = _scenarioFromStream( - stream=stream, - compileOptions=scenarioCreationData["compileOptions"], - filename=scenarioCreationData["filename"], - scenario=scenarioCreationData["scenario"], - path=scenarioCreationData["path"], - _cacheImports=False, - ) - - while True: - seed = seedQueue.get() - setSeed(seed) - - scene, iterations = scenario._generateInner( - maxIterations=float("inf"), verbosity=verbosity, feedback=None - ) - sceneBytes = scenario.sceneToBytes(scene) - - sceneQueue.put((sceneBytes, iterations, seed)) diff --git a/tests/core/test_scenarios.py b/tests/core/test_scenarios.py index e4365d462..d77c133a0 100644 --- a/tests/core/test_scenarios.py +++ b/tests/core/test_scenarios.py @@ -74,6 +74,7 @@ def test_generateBatch(): ) scenes, _ = scenario.generateBatch(2, numWorkers=2) + assert len(scenes) == 2 assert all(isinstance(scene, Scene) for scene in scenes) assert all(0 <= scene.objects[0].heading <= 1 for scene in scenes) assert scenes[0].objects[0].heading != scenes[1].objects[0].heading @@ -87,6 +88,7 @@ def test_generateBatch_serialized(): """ ) scenesBytes, _ = scenario.generateBatch(2, numWorkers=2, serialized=True) + assert len(scenesBytes) == 2 assert all(isinstance(b, bytes) for b in scenesBytes) scenes = [scenario.sceneFromBytes(b, verify=True) for b in scenesBytes] @@ -108,6 +110,7 @@ def test_generateStream_deterministic(): streamA = tuple(scenario.generateStream(8, numWorkers=2, serialized=True)) setSeed(seed) streamB = tuple(scenario.generateStream(8, numWorkers=2, serialized=True)) + assert len(streamA) == len(streamB) == 8 bytesSetA = {result[0] for result in streamA} bytesSetB = {result[0] for result in streamB} assert bytesSetA == bytesSetB diff --git a/tools/benchmarking/parallelization/benchmark_parallelization.py b/tools/benchmarking/parallelization/benchmark_scene_parallel.py similarity index 73% rename from tools/benchmarking/parallelization/benchmark_parallelization.py rename to tools/benchmarking/parallelization/benchmark_scene_parallel.py index fec0f3191..27b2b64fb 100644 --- a/tools/benchmarking/parallelization/benchmark_parallelization.py +++ b/tools/benchmarking/parallelization/benchmark_scene_parallel.py @@ -52,28 +52,28 @@ def run_benchmark_parallel(path, params, *, numWorkers): scenario = scenic.scenarioFromFile( BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) ) - scenario.generateBatch(NUM_SAMPLES, maxIterations=float("inf"), numWorkers=numWorkers) + scenario.generateBatch(NUM_SAMPLES, numWorkers=numWorkers) if __name__ == "__main__": - print("Base Performance (`generate`, numWorkers=0):") - for benchmark in BENCHMARKS: - with warnings.catch_warnings(): - warnings.simplefilter("ignore") - start = time.time() - run_benchmark(*benchmark) - trial_time = time.time() - start - print(f"{trial_time: 7.2f} | {benchmark}") - print() - print("Base + Overhead Performance (`generateBatch`, numWorkers=1):") - for benchmark in BENCHMARKS: - with warnings.catch_warnings(): - warnings.simplefilter("ignore") - start = time.time() - run_benchmark_parallel(*benchmark, numWorkers=1) - trial_time = time.time() - start - print(f"{trial_time: 7.2f} | {benchmark}") - print() + # print("Base Performance (`generate`, numWorkers=0):") + # for benchmark in BENCHMARKS: + # with warnings.catch_warnings(): + # warnings.simplefilter("ignore") + # start = time.time() + # run_benchmark(*benchmark) + # trial_time = time.time() - start + # print(f"{trial_time: 7.2f} | {benchmark}") + # print() + # print("Base + Overhead Performance (`generateBatch`, numWorkers=1):") + # for benchmark in BENCHMARKS: + # with warnings.catch_warnings(): + # warnings.simplefilter("ignore") + # start = time.time() + # run_benchmark_parallel(*benchmark, numWorkers=1) + # trial_time = time.time() - start + # print(f"{trial_time: 7.2f} | {benchmark}") + # print() print("Batch Performance (`generateBatch`, numWorkers=8):") for benchmark in BENCHMARKS: with warnings.catch_warnings(): diff --git a/tools/benchmarking/parallelization/benchmark_sim_parallel.py b/tools/benchmarking/parallelization/benchmark_sim_parallel.py new file mode 100644 index 000000000..4d373b013 --- /dev/null +++ b/tools/benchmarking/parallelization/benchmark_sim_parallel.py @@ -0,0 +1,89 @@ +from pathlib import Path +import time +import warnings + +import scenic +from scenic.core.utils import SimulatorGroup +from scenic.simulators.metadrive.simulator import MetaDriveSimulator + +NUM_WORKERS = 8 +BENCHMARKS_BASE_PATH = (Path(__file__).resolve().parent / "benchmarks").resolve() +MAP_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent + / "assets" + / "maps" + / "CARLA" + / "Town05.xodr" +).resolve() +SUMO_MAP_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent + / "assets" + / "maps" + / "CARLA" + / "Town05.net.xml" +).resolve() +MESH_BASE_PATH = ( + Path(__file__).resolve().parent.parent.parent.parent / "assets" / "meshes" +).resolve() + +BENCHMARKS = [ + ("badlyParkedCarPullingIn.scenic", {"mode2D": True, "map": MAP_PATH}), + ("bypassing_03.scenic", {"mode2D": True, "map": MAP_PATH}), +] + +NUM_SAMPLES = 128 + + +def run_benchmark(path, params): + simulator = MetaDriveSimulator(sumo_map=SUMO_MAP_PATH, render=False, real_time=False) + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + for _ in range(NUM_SAMPLES): + scene, _ = scenario.generate(maxIterations=float("inf")) + simulator.simulate(scene) + + +def run_benchmark_parallel(path, params, *, numWorkers): + scenario = scenic.scenarioFromFile( + BENCHMARKS_BASE_PATH / path, params=params, mode2D=params.get("mode2D", False) + ) + scene_stream = scenario.generateStream( + NUM_SAMPLES, numWorkers=numWorkers, serialized=True + ) + sim_group = SimulatorGroup( + numWorkers=numWorkers, + simulatorClass=MetaDriveSimulator, + simulatorParams={"sumo_map": SUMO_MAP_PATH, "render": False, "real_time": False}, + mute=False, + ) + sim_group.simulateBatch(scenario=scenario, scenes=scene_stream) + + +if __name__ == "__main__": + print("Base Performance (`generate`, numWorkers=0):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark(*benchmark) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") + print() + print("Base + Overhead Performance (`generateBatch`, numWorkers=1):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=1) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") + print() + print("Batch Performance (`generateBatch`, numWorkers=8):") + for benchmark in BENCHMARKS: + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + start = time.time() + run_benchmark_parallel(*benchmark, numWorkers=8) + trial_time = time.time() - start + print(f"{trial_time: 7.2f} | {benchmark}") diff --git a/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic b/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic index 9e1b15b18..ba566dc73 100644 --- a/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic +++ b/tools/benchmarking/parallelization/benchmarks/badlyParkedCarPullingIn.scenic @@ -1,6 +1,6 @@ param time_step = 1.0/10 -model scenic.domains.driving.model +model scenic.simulators.metadrive.model behavior PullIntoRoad(): while (distance from self to ego) > 15: diff --git a/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic b/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic index ebbab8023..647f90ed5 100644 --- a/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic +++ b/tools/benchmarking/parallelization/benchmarks/bypassing_03.scenic @@ -12,7 +12,7 @@ SOURCE: NHSTA, #16 # MAP AND MODEL # ################################# -model scenic.simulators.carla.model +model scenic.simulators.metadrive.model ################################# # CONSTANTS # @@ -104,3 +104,4 @@ require (distance from adversary to intersection) > INIT_DIST require (distance from lead to intersection) > INIT_DIST require always (adversary.laneSection._fasterLane is not None) terminate when (distance to egoSpawnPt) > TERM_DIST +terminate after 60 seconds From 8ecd602149f8f06b6ae299f344603a2ed81dfffb Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 15 Dec 2025 18:45:02 -0800 Subject: [PATCH 011/134] Added pedestrians to OpenScenarioXML export --- src/scenic/core/serialization.py | 96 +++++++++++++++---------- src/scenic/domains/driving/model.scenic | 8 +++ 2 files changed, 66 insertions(+), 38 deletions(-) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index b4a66d516..bba11549d 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -385,6 +385,7 @@ def toOpenScenario( maxSpeed=69, maxAcceleration=10, maxDeceleration=10, + pedestrianMass=65, ): # Create catalog xosc_catalog = xosc.Catalog() @@ -401,46 +402,65 @@ def toOpenScenario( entities = xosc.Entities() xosc_objects = {} for obj_i, obj in enumerate(scene.objects): - if not hasattr(obj, "isVehicle") or not obj.isVehicle: - warnings.warn("Non-vehicle object {} is ignored.") + if hasattr(obj, "isVehicle") and obj.isVehicle: + obj_name = obj.name if hasattr(obj, "name") else f"Vehicle{obj_i}" + veh_bb = xosc.BoundingBox( + obj.width, + obj.length, + obj.height, + 0, + 0, + 0, + ) + veh_fa = xosc.Axle( + maxSteeringAngle, + wheelDiameter, + trackWidth, + wheelbaseRatio * obj.length, + groundClearance, + ) + veh_ra = xosc.Axle( + maxSteeringAngle, wheelDiameter, trackWidth, 0, groundClearance + ) + xosc_obj = xosc.Vehicle( + name=obj_name, + vehicle_type=xosc.VehicleCategory.car, + boundingbox=veh_bb, + frontaxle=veh_fa, + rearaxle=veh_ra, + max_speed=maxSpeed, + max_acceleration=maxAcceleration, + max_deceleration=maxDeceleration, + mass=None, + model3d=None, + max_acceleration_rate=None, + max_deceleration_rate=None, + role=None, + ) + elif hasattr(obj, "isPedestrian") and obj.isPedestrian: + obj_name = obj.name if hasattr(obj, "name") else f"Pedestrian{obj_i}" + ped_bb = xosc.BoundingBox( + obj.width, + obj.length, + obj.height, + 0, + 0, + 0, + ) + xosc_obj = xosc.Pedestrian( + name=obj_name, + mass=pedestrianMass, + boundingbox=ped_bb, + category=xosc.PedestrianCategory.pedestrian, + model=None, + role=None, + ) + else: + warnings.warn(f"Unknown object {obj} is ignored.") continue - veh_name = obj.name if hasattr(obj, "name") else f"Vehicle{obj_i}" - veh_bb = xosc.BoundingBox( - obj.width, - obj.length, - obj.height, - 0, - 0, - 0, - ) - veh_fa = xosc.Axle( - maxSteeringAngle, - wheelDiameter, - trackWidth, - wheelbaseRatio * obj.length, - groundClearance, - ) - veh_ra = xosc.Axle( - maxSteeringAngle, wheelDiameter, trackWidth, 0, groundClearance - ) - xosc_veh = xosc.Vehicle( - name=veh_name, - vehicle_type=xosc.VehicleCategory.car, - boundingbox=veh_bb, - frontaxle=veh_fa, - rearaxle=veh_ra, - max_speed=maxSpeed, - max_acceleration=maxAcceleration, - max_deceleration=maxDeceleration, - mass=None, - model3d=None, - max_acceleration_rate=None, - max_deceleration_rate=None, - role=None, - ) - xosc_objects[obj] = xosc_veh - entities.add_scenario_object(veh_name, xosc_veh) + xosc_objects[obj] = xosc_obj + entities.add_scenario_object(obj_name, xosc_obj) # Create init init = xosc.Init() diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index b6d0754af..4a3219a56 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -133,6 +133,10 @@ class DrivingObject: def isVehicle(self): return False + @property + def isPedestrian(self): + return False + @property def isCar(self): return False @@ -325,6 +329,10 @@ class Pedestrian(DrivingObject): length: 0.75 color: [0, 0.5, 1] + @property + def isPedestrian(self): + return True + ## Utility functions def withinDistanceToAnyCars(car, thresholdDistance): From 349727ffacde8a8055e3efbae0e2f686ddffd850 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 24 Dec 2025 20:55:48 -0800 Subject: [PATCH 012/134] Added documentation for setSeed. --- docs/api.rst | 7 +++++++ src/scenic/core/utils.py | 1 + 2 files changed, 8 insertions(+) diff --git a/docs/api.rst b/docs/api.rst index 440f450f2..d6d2b504b 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -40,6 +40,13 @@ the sampled values for all the global parameters and objects in the scene from t ego has foo = 2.083099362726706 +Utilities +--------- + +Scenic provides top level utility functions for functions like setting Scenic's random seed. + +.. autofunction:: scenic.setSeed + Running Dynamic Simulations --------------------------- diff --git a/src/scenic/core/utils.py b/src/scenic/core/utils.py index 5266af78d..f01d58b2c 100644 --- a/src/scenic/core/utils.py +++ b/src/scenic/core/utils.py @@ -399,5 +399,6 @@ def get_type_hints(obj, globalns=None, localns=None): def setSeed(seed): + """Set the random seed used by Scenic""" random.seed(seed) numpy.random.seed(seed) From 335b437756c32cf3c5a70aa8712e05de24c08a17 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 25 Dec 2025 11:14:52 -0800 Subject: [PATCH 013/134] Added documentation and tests for simulation parallelization. --- src/scenic/core/scenarios.py | 24 ++++---- src/scenic/core/simulators.py | 39 ++++++++++++- tests/core/test_simulators.py | 105 +++++++++++++++++++++++++++++++++- 3 files changed, 156 insertions(+), 12 deletions(-) diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index 852680ada..2476a4653 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -480,19 +480,19 @@ def generateStream( """Sample a stream of `Scene` objects from this scenario. For a description of how scene generation is done, see `scene generation`. This function can produce - both finite and infinite streams (depending on the value of `numScenes`). + both finite and infinite streams (depending on the value of ``numScenes``). .. note:: - NOTE: The deterministic parameter is by default set to True, meaning that scenes + NOTE: The ``deterministic`` parameter is by default set to True, meaning that scenes will be returned in a fixed order for a given Scenic seed. Setting this to False means scenes will be returned in a possibly non-deterministic order, but with possibly - decreased latency. When deterministic is set to False, the ordering of the returned scenes + decreased latency. When ``deterministic`` is set to ``False``, the ordering of the returned scenes is not fully random, with scenes that are easier to generate being more likely to be returned earlier in the stream. Despite this, the overall distribution of the returned scenes still - matches the scenario. When generating an infinite stream, `deterministic` must be set to True. + matches the scenario. When generating an infinite stream, ``deterministic`` must be set to ``True``. Args: - numScenes (int): Number of scenes to generate, or `float('inf')` to sample an infinite stream + numScenes (int): Number of scenes to generate, or ``float('inf')`` to sample an infinite stream of Scenes. maxIterations (int): Maximum number of rejection sampling iterations (over all scenes). verbosity (int): Verbosity level. @@ -501,9 +501,9 @@ def generateStream( numWorkers (int): The number of workers to be used when generating scenes. If numWorkers is 0, scenes will be generated in the main process. bufferSize (int): The number of scenes to have available at any given time, or `None` to - use default values. If set to `None` and `numScenes` is finite, all scenes are generated - in a greedy fashon. If set to `None and `numScenes` is infinite, set to a default - value of `2*numWorkers`. + use default values. If set to ``None`` and ``numScenes`` is finite, all scenes are generated + in a greedy fashon. If set to ``None`` and ``numScenes`` is infinite, set to a default + value of ``2*numWorkers``. mute (bool): Whether or not to mute stdOut and stdErr in the worker processes. serialized (bool): Whether or not to return scenes in a serialized format. deterministic (bool): Whether or not scenes will be returned in a deterministic order. This @@ -536,11 +536,15 @@ def generateStream( while returnedResultCount < numScenes: try: remainingIts = maxIterations - totalIterations - scene, iterations = self._generateInner( + rawScene, iterations = self._generateInner( remainingIts, verbosity, feedback ) + scene = self.sceneToBytes(rawScene) if serialized else rawScene totalIterations += iterations - yield (scene, iterations) + if iterationCount: + yield (scene, iterations) + else: + yield scene returnedResultCount += 1 except RejectionException: raise RejectionException( diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index aa550ed1c..1076c5634 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -959,6 +959,23 @@ def __init__( class SimulatorGroup: + """A group of simulators for running parallel simulations. + + Args: + numWorkers: Number of workers in this group. + simulatorClass: The simulator class this group is composed of. + simulatorParams: An optional single or list of kwarg dictionaries to be passed as parameters + when creating the simulators in this group. If simulatorParams is a list of dicts, + ``len(simulatorParams)`` should equal ``numWorkers``. + bufferSize: An optional integer indicating the size of the job buffer. If ``None``, the value is + set to ``2 * numWorkers``. + mute: Whether or not to mute stdOut and stdErr in the worker processes. + returnFinalState: Whether or not returned `SimulationResult` objects should contain the ``finalState`` + property. Set to ``False`` by default to minimize overhead. + returnTrajectory: Whether or not returned `SimulationResult` objects should contain the ``trajectry`` + property. Set to ``False`` by default to minimize overhead. + """ + def __init__( self, numWorkers, @@ -1008,7 +1025,7 @@ def _serializeScene(self, scene, scenario, serialized): raise ValueError( f"Scene provided has type `{type(scene)}` instead of type `Scene`, but serialized was set to False." ) - return scenario.sceneFromBytes(scene, verify=True) + return scenario.sceneToBytes(scene) def _prepareJob( self, scene, simulateParams, jobId, scenario, serialized, rand_generator @@ -1028,6 +1045,14 @@ def _prepareJob( return (jobId, serializedScene, jobParams, seed) def simulateBatch(self, scenario, scenes, simulateParams=None, serialized=True): + """Simulate and return a batch of `SimulationResult` objects. + + Args: + scenario: The scenario that the scenes are sampled from. + scenes: An iterator of `Scene` objects sampled from ``scenario``. + simulateParams: An optional dictionary of params to be passed to simulate internally. + serialized: Whether or not ``scenes`` contains serialized scenes. + """ return tuple( v[1] for v in self.simulateStream( @@ -1038,6 +1063,18 @@ def simulateBatch(self, scenario, scenes, simulateParams=None, serialized=True): def simulateStream( self, scenario, scenes, simulateParams=None, serialized=True, deterministic=False ): + """Generate a stream of `SimulationResult` objects. + + Args: + scenario: The scenario that the scenes are sampled from. + scenes: An iterator of `Scene` objects sampled from ``scenario``. + simulateParams: An optional dictionary of params to be passed to simulate internally. + serialized: Whether or not ``scenes`` contains serialized scenes. + deterministic: Whether or not results should be returned in a deterministic order. Setting + this to ``False`` can result in decreased latency when accessing results, but the order + will not be fixed. + """ + scenes = iter(scenes) simulateParams = simulateParams if simulateParams else dict() # Create helper parameters diff --git a/tests/core/test_simulators.py b/tests/core/test_simulators.py index 149c1cad1..1b22aad5e 100644 --- a/tests/core/test_simulators.py +++ b/tests/core/test_simulators.py @@ -1,6 +1,15 @@ +import itertools +import random + import pytest -from scenic.core.simulators import DummySimulation, DummySimulator, Simulation +import scenic +from scenic.core.simulators import ( + DummySimulation, + DummySimulator, + Simulation, + SimulatorGroup, +) from tests.utils import compileScenic, sampleResultFromScene, sampleSceneFrom @@ -94,3 +103,97 @@ class TestObj: simulator = TestSimulator() with pytest.raises(RuntimeError): result = simulator.simulate(scene, maxSteps=2) + + +@pytest.mark.slow +def test_simulator_group(): + scenario = compileScenic( + """ + behavior Foo(): + while True: + require Range(0,1) < 0.99 + wait + + new Object with behavior Foo() + """ + ) + + for numWorkers, serialized, scene_stream, sim_stream in itertools.product( + [1, 2], [True, False], [True, False], [True, False] + ): + if scene_stream: + scenes = scenario.generateStream( + 200, numWorkers=numWorkers, serialized=serialized, iterationCount=False + ) + else: + scenes, _ = scenario.generateBatch( + 200, numWorkers=numWorkers, serialized=serialized + ) + + sim_group = SimulatorGroup( + numWorkers=2, simulatorClass=DummySimulator, mute=False + ) + + simulate_params = {"maxSteps": 10} + + if sim_stream: + results = tuple( + result + for _, result in sim_group.simulateStream( + scenario, + scenes, + simulateParams=simulate_params, + serialized=serialized, + ) + ) + else: + results = sim_group.simulateBatch( + scenario, scenes, simulateParams=simulate_params, serialized=serialized + ) + + assert any(val is None for val in results) + assert any(val is not None for val in results) + + +def test_simulator_group_deterministic(): + scenario = compileScenic( + """ + behavior Foo(): + while True: + require Range(0,1) < 0.99 + wait + + new Object with behavior Foo() + """ + ) + + seed = random.getrandbits(32) + + scenic.setSeed(seed) + scenes, _ = scenario.generateBatch(200, serialized=True) + + sim_group = SimulatorGroup(numWorkers=4, simulatorClass=DummySimulator, mute=False) + simulate_params = {"maxSteps": 10} + + results1 = tuple( + result + for _, result in sim_group.simulateStream( + scenario, scenes, simulateParams=simulate_params, deterministic=True + ) + ) + + scenic.setSeed(seed) + scenes, _ = scenario.generateBatch(200, serialized=True) + + sim_group = SimulatorGroup(numWorkers=4, simulatorClass=DummySimulator, mute=False) + simulate_params = {"maxSteps": 10} + + results2 = tuple( + result + for _, result in sim_group.simulateStream( + scenario, scenes, simulateParams=simulate_params, deterministic=True + ) + ) + + assert len(results1) == len(results2) + assert all((v1 is None) == (v2 is None) for v1, v2 in zip(results1, results2)) From f673bb466ea25c9a0acf82c35ce0eedc0127d06f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sat, 18 Apr 2026 13:09:36 -0700 Subject: [PATCH 014/134] Minor documentation and test additions. --- docs/api.rst | 4 ++-- docs/options.rst | 2 ++ tests/syntax/test_basic.py | 13 +++++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/docs/api.rst b/docs/api.rst index d6d2b504b..33fbbed77 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -30,8 +30,8 @@ the sampled values for all the global parameters and objects in the scene from t .. testcode:: - import random, scenic - random.seed(12345) + import scenic + scenic.setSeed(12345) scenario = scenic.scenarioFromString('ego = new Object with foo Range(0, 5)') scene, numIterations = scenario.generate() print(f'ego has foo = {scene.egoObject.foo}') diff --git a/docs/options.rst b/docs/options.rst index 1df985596..6a4d7ab2c 100644 --- a/docs/options.rst +++ b/docs/options.rst @@ -48,6 +48,8 @@ General Scenario Control (although :mod:`random` and :mod:`numpy.random` should not be used in place of Scenic's own sampling constructs in Scenic code). + The seed can be set programatically using `scenic.setSeed`. + .. option:: --scenario If the given Scenic file defines multiple scenarios, select which one to run. diff --git a/tests/syntax/test_basic.py b/tests/syntax/test_basic.py index dd022b208..55283d56a 100644 --- a/tests/syntax/test_basic.py +++ b/tests/syntax/test_basic.py @@ -330,3 +330,16 @@ class Foo(Bar): obj = sampleEgoFrom(program, mode2D=True) assert obj.heading == obj.parentOrientation.yaw == 0.56 + + +def test_setSeed(): + scenario = compileScenic("ego = new Object with foo Range(0,1)") + + scenic.setSeed(10) + s1, _ = scenario.generate() + scenic.setSeed(10) + s2, _ = scenario.generate() + s3, _ = scenario.generate() + + assert s1.objects[0].foo == s2.objects[0].foo + assert s1.objects[0].foo != s3.objects[0].foo From 24e5ee934e5b4f0df5fce771db95f5c6f3255333 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sat, 18 Apr 2026 13:11:23 -0700 Subject: [PATCH 015/134] Simplified test. --- tests/syntax/test_basic.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/tests/syntax/test_basic.py b/tests/syntax/test_basic.py index 55283d56a..24694cc38 100644 --- a/tests/syntax/test_basic.py +++ b/tests/syntax/test_basic.py @@ -333,13 +333,11 @@ class Foo(Bar): def test_setSeed(): - scenario = compileScenic("ego = new Object with foo Range(0,1)") - scenic.setSeed(10) - s1, _ = scenario.generate() + p1 = sampleParamPFrom("param p = Range(0, 1)") scenic.setSeed(10) - s2, _ = scenario.generate() - s3, _ = scenario.generate() + p2 = sampleParamPFrom("param p = Range(0, 1)") + p3 = sampleParamPFrom("param p = Range(0, 1)") - assert s1.objects[0].foo == s2.objects[0].foo - assert s1.objects[0].foo != s3.objects[0].foo + assert p1 == p2 + assert p1 != p3 From 5e2b7e33d84c428b26212033fa49cb28c224fedf Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 28 May 2026 20:07:44 -0700 Subject: [PATCH 016/134] XOSC export improvements, fixes, and test. --- src/scenic/core/serialization.py | 80 ++++++++++---------- src/scenic/domains/driving/model.scenic | 28 +++++++ tests/core/test_serialization.py | 50 +++++++++++- tests/simulators/metadrive/test_metadrive.py | 5 +- 4 files changed, 120 insertions(+), 43 deletions(-) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 0dab3290c..ab7058a7e 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -403,15 +403,6 @@ def toOpenScenario( simulationResult, mapPath=None, scenarioName="ScenicScenario", - wheelbaseRatio=0.6, - maxSteeringAngle=0.523598775598, - wheelDiameter=0.8, - trackWidth=1.68, - groundClearance=0.4, - maxSpeed=69, - maxAcceleration=10, - maxDeceleration=10, - pedestrianMass=65, ): try: import scenariogeneration @@ -447,14 +438,18 @@ def toOpenScenario( 0, ) veh_fa = xosc.Axle( - maxSteeringAngle, - wheelDiameter, - trackWidth, - wheelbaseRatio * obj.length, - groundClearance, + obj.maxSteeringAngle, + obj.wheelDiameter, + obj.trackWidth, + obj.wheelbase, + obj.groundClearance, ) veh_ra = xosc.Axle( - maxSteeringAngle, wheelDiameter, trackWidth, 0, groundClearance + obj.maxSteeringAngle, + obj.wheelDiameter, + obj.trackWidth, + 0, + obj.groundClearance, ) xosc_obj = xosc.Vehicle( name=obj_name, @@ -462,9 +457,9 @@ def toOpenScenario( boundingbox=veh_bb, frontaxle=veh_fa, rearaxle=veh_ra, - max_speed=maxSpeed, - max_acceleration=maxAcceleration, - max_deceleration=maxDeceleration, + max_speed=obj.maxSpeed, + max_acceleration=obj.maxAcceleration, + max_deceleration=obj.maxDeceleration, mass=None, model3d=None, max_acceleration_rate=None, @@ -483,35 +478,45 @@ def toOpenScenario( ) xosc_obj = xosc.Pedestrian( name=obj_name, - mass=pedestrianMass, + mass=obj.mass, boundingbox=ped_bb, category=xosc.PedestrianCategory.pedestrian, model=None, role=None, ) else: - warnings.warn(f"Unknown object {obj} is ignored.") + warnings.warn( + f"Object {obj} of unsupported type is being ignored during XOSC export." + ) continue xosc_objects[obj] = xosc_obj entities.add_scenario_object(obj_name, xosc_obj) - # Create init - init = xosc.Init() - - for obj, xosc_obj in xosc_objects.items(): - scenic_yaw = obj.yaw - state_orientation = scenic_yaw + math.radians(90) - state_position = obj.position.offsetRotated( - scenic_yaw, Vector(0, -0.5 * wheelbaseRatio * obj.length, 0) + # Helper function + def pos_to_WorldPosition(obj, pos, yaw): + # XOSC Reference point is back axle, so we must translate Scenic's + # convention to this. + state_position = ( + pos.offsetRotated(yaw, Vector(0, -0.5 * obj.wheelbase, 0)) + if obj.isVehicle + else pos ) - init_position = xosc.WorldPosition( + state_orientation = yaw + math.radians(90) + return xosc.WorldPosition( x=state_position.x, y=state_position.y, z=state_position.z, h=state_orientation, ) - obj_init_action = xosc.TeleportAction(init_position) + + # Initial states + init = xosc.Init() + + for obj, xosc_obj in xosc_objects.items(): + obj_init_action = xosc.TeleportAction( + pos_to_WorldPosition(obj, obj.position, obj.yaw) + ) init.add_init_action(xosc_obj.name, obj_init_action) # Dynamics @@ -529,19 +534,12 @@ def toOpenScenario( action_times = [] action_positions = [] for t, states in enumerate(simulationResult.trajectory): - scenic_yaw = states.orientations[obj_i].yaw - state_orientation = scenic_yaw + math.radians(90) - state_position = states.positions[obj_i].offsetRotated( - scenic_yaw, Vector(0, -0.5 * wheelbaseRatio * obj.length, 0) + action_positions.append( + pos_to_WorldPosition( + obj, states.positions[obj_i], states.orientations[obj_i].yaw + ) ) action_times.append(simulationResult.timestep * t) - pos = xosc.WorldPosition( - x=state_position.x, - y=state_position.y, - z=state_position.z, - h=state_orientation, - ) - action_positions.append(pos) polyline = xosc.Polyline(time=action_times, positions=action_positions) trajectory = xosc.Trajectory(name=f"Trajectory_{xosc_obj.name}", closed=False) diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index 7652b43cb..8db0c2805 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -286,6 +286,24 @@ class Vehicle(DrivingObject): color (:obj:`Color` or RGB tuple): Color of the vehicle. The default value is a distribution derived from car color popularity statistics; see :obj:`Color.defaultCarColor`. + wheelbase: The distance between the front and rear axles of the vehicle. Default value is 0.6 + times the length of the vehicle. + maxSteeringAngle: The maximum steering angle of the vehicle. The full steering range would be + two times this value, going from (-maxSteeringAngle, maxSteeringAngle). Default value + 30 degrees. + wheelDiameter: The diameter of the *entire* wheel (including the tire). Default value is 0.7 meters. + trackWidth: Distance between the vehicle's wheels when pointed straight ahead. Default value + is 0.85 times the width of the vehicle. + groundClearance: Default value is half the wheel diameter. + maxSpeed: The maximum rated speed of the vehicle. Default value is 45 meters per second (~100 mph). + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). + maxAcceleration: The maximum rated acceleration of the vehicle. Default value is 5 meters per second^2. + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). + maxDeceleration: The maximum rated deceleration of the vehicle. Default value is 10 meters per second^2. + This value is not enforced by Scenic and is provided simply for other tools to reference (e.g. + exporting to OpenScenarioXML). """ regionContainedIn: roadOrShoulder position: new Point on road @@ -295,6 +313,14 @@ class Vehicle(DrivingObject): width: 2 length: 4.5 color: Color.defaultCarColor() + wheelbase: 0.6*self.length + maxSteeringAngle: 35 deg + wheelDiameter: 0.7 + trackWidth: 0.85*self.width + groundClearance: 0.5*self.wheelDiameter + maxSpeed: 45 + maxAcceleration: 5 + maxDeceleration: 10 @property def isVehicle(self): @@ -321,6 +347,7 @@ class Pedestrian(DrivingObject): length: The default length is 0.75 m. color: The default color is turquoise. Pedestrian colors are not necessarily used by simulators, but do appear in the debugging diagram. + mass: Default value is 65 kg. """ regionContainedIn: network.walkableRegion position: new Point on network.walkableRegion @@ -329,6 +356,7 @@ class Pedestrian(DrivingObject): width: 0.75 length: 0.75 color: [0, 0.5, 1] + mass: 65 @property def isPedestrian(self): diff --git a/tests/core/test_serialization.py b/tests/core/test_serialization.py index 6ae1d1c95..e7b88ccc2 100644 --- a/tests/core/test_serialization.py +++ b/tests/core/test_serialization.py @@ -13,8 +13,14 @@ import numpy import pytest -from scenic.core.serialization import SerializationError, Serializer, deterministicHash +from scenic.core.serialization import ( + SerializationError, + Serializer, + deterministicHash, + toOpenScenario, +) from scenic.core.simulators import DivergenceError, DummySimulator +from tests.simulators.metadrive.test_metadrive import getMetadriveSimulator from tests.utils import ( areEquivalent, compileScenic, @@ -507,3 +513,45 @@ class Foo: digest2 = deterministicHash(mapping2) # Non-scalar values should hash in a stable way, independent of identity. assert digest1 == digest2 + + +def test_xosc_export(getMetadriveSimulator): + simulator, openDrivePath, sumoPath = getMetadriveSimulator("Town01") + code = f""" + param map = r'{openDrivePath}' + param sumo_map = r'{sumoPath}' + + model scenic.simulators.metadrive.model + + behavior DriveAndBrakeForPedestrians(): + try: + do FollowLaneBehavior() + interrupt when withinDistanceToAnyPedestrians(self, 10): + take SetThrottleAction(0), SetBrakeAction(1) + + behavior CrossRoad(): + while distance from self to ego > 15: + wait + take SetWalkingDirectionAction(self.heading), SetWalkingSpeedAction(1) + + ego = new Car with behavior DriveAndBrakeForPedestrians() + + rightCurb = ego.laneGroup.curb + spot = new OrientedPoint on visible rightCurb + + parkedCar = new Car right of spot by 1, with regionContainedIn None + + require distance from ego to parkedCar > 30 + + new Pedestrian ahead of parkedCar by 3, + facing 90 deg relative to parkedCar, + with behavior CrossRoad() + + terminate after 30 seconds + """ + + scenario = compileScenic(code, mode2D=True, params={"map": openDrivePath}) + scene, _ = scenario.generate() + simulationResult = simulator.simulate(scene) + assert simulationResult is not None + xosc_scenario = toOpenScenario(scenario, scene, simulationResult) diff --git a/tests/simulators/metadrive/test_metadrive.py b/tests/simulators/metadrive/test_metadrive.py index 1c1ea9ad1..4fe573d2e 100644 --- a/tests/simulators/metadrive/test_metadrive.py +++ b/tests/simulators/metadrive/test_metadrive.py @@ -84,7 +84,9 @@ def test_pickle(loadLocalScenario): def getMetadriveSimulator(getAssetPath): base = getAssetPath("maps/CARLA") - def _getMetadriveSimulator(town, *, render=False, render3D=False, **kwargs): + def _getMetadriveSimulator( + town, *, render=False, render3D=False, real_time=False, **kwargs + ): openDrivePath = os.path.join(base, f"{town}.xodr") sumoPath = os.path.join(base, f"{town}.net.xml") simulator = MetaDriveSimulator( @@ -92,6 +94,7 @@ def _getMetadriveSimulator(town, *, render=False, render3D=False, **kwargs): xodr_map=openDrivePath, render=render, render3D=render3D, + real_time=real_time, **kwargs, ) return simulator, openDrivePath, sumoPath From 743cf43ce3f61800acc2f3abdcc33a6b1fee6a96 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 28 May 2026 20:10:51 -0700 Subject: [PATCH 017/134] Fixed metadrive import? --- pyproject.toml | 1 - 1 file changed, 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 2aace9537..6f6cdf907 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -71,7 +71,6 @@ test = [ # minimum dependencies for running tests (used for tox virtualenvs) test-full = [ # like 'test' but adds dependencies for optional features "scenic[test]", # all dependencies from 'test' extra above "scenic[guideways]", # for running guideways modules - "scenic[metadrive]", "scenic[openscenario]", "astor >= 0.8.1", 'carla >= 0.9.12; python_version <= "3.12" and (platform_system == "Linux" or platform_system == "Windows")', From 992be2554b0519fe112ee5ad2ecb01d3ff68c1cd Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 28 May 2026 20:16:07 -0700 Subject: [PATCH 018/134] Tweaked Metadrive real_time default. --- src/scenic/simulators/metadrive/simulator.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 76607d5bc..075134f55 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -39,7 +39,7 @@ def __init__( timestep=0.1, render=True, render3D=False, - real_time=True, + real_time=None, screen_record=False, screen_record_filename=None, screen_record_path="metadrive_gifs", @@ -51,7 +51,10 @@ def __init__( self.timestep = timestep self.sumo_map = sumo_map self.xodr_map = xodr_map - self.real_time = real_time + if real_time is None: + self.real_time = self.render or self.render3D + else: + self.real_time = real_time self.screen_record = screen_record self.screen_record_filename = screen_record_filename self.screen_record_path = screen_record_path From 15c729a59c114340a3afd1f9830c2b894951b337 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 28 May 2026 20:38:41 -0700 Subject: [PATCH 019/134] Added documentation --- docs/api.rst | 7 +++++++ docs/simulators.rst | 5 ++++- src/scenic/core/serialization.py | 22 +++++++++++++++++++--- tests/core/test_serialization.py | 2 +- 4 files changed, 31 insertions(+), 5 deletions(-) diff --git a/docs/api.rst b/docs/api.rst index 440f450f2..5cfe99f09 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -173,6 +173,13 @@ it to the replay, but this greatly increases the size of the encoded simulation. can return to one later for further analysis), but it is not guaranteed to be compatible across major versions of Scenic. +.. _xosc_export: +OpenScenarioXML Export +---------------------- +Scenic provides experimental support for exporting completed simulations via `toOpenScenario`. +This function currently only supports cars and pedestrians, and may be subject to breaking changes +in the future. + .. seealso:: If you get exceptions or unexpected behavior when using the API, Scenic provides various debugging features: see :ref:`debugging`. .. rubric:: Footnotes diff --git a/docs/simulators.rst b/docs/simulators.rst index ee5d0b0d1..5a966660b 100644 --- a/docs/simulators.rst +++ b/docs/simulators.rst @@ -14,6 +14,10 @@ See the individual entries for details on each interface's capabilities and how While Scenic aims to support multiple Python versions, some simulators may have more limited compatibility. Be sure to check the documentation of each simulator to confirm which Python versions are supported. +.. note:: + Scenic also supports outputing data in formats that may be imported into other simulators and tools (e.g. :ref:`xosc_export`). + For more details, see :ref:`serialization`. + .. contents:: List of Simulators :local: @@ -163,7 +167,6 @@ This interface is part of the VerifAI toolkit; documentation and examples can be .. _VerifAI repository: https://github.com/BerkeleyLearnVerify/VerifAI - Deprecated ========== diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index ab7058a7e..77fb68938 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -398,12 +398,22 @@ def readStr(stream): def toOpenScenario( + simulationResult, scenario, scene, - simulationResult, mapPath=None, scenarioName="ScenicScenario", ): + """Export a `SimulationResult` as a `scenariogeneration` `xosc` object. + + Args: + simulationResult: The `SimulationResult` to be exported to XOSC + scenario: The scenario from which simulationResult was sampled. + scene: The scene from which simulationResult was sampled. + mapPath: The path to the XODR map used to run the simulation. If + one is not provided the `map` param of the scenario is used. + scenarioName: The name of the scenario in the generated XOSC file. + """ try: import scenariogeneration from scenariogeneration import ScenarioGenerator, xosc @@ -419,8 +429,14 @@ def toOpenScenario( xosc_paramdec = xosc.ParameterDeclarations() # Extract map - assert "map" in scenario.params - map_path = mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) + if map_path is None: + if "map" not in scenario.params: + raise ValueError( + "No `mapPath` provided and scenario does not have a `map` parameter defined." + ) + map_path = ( + mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) + ) xosc_road = xosc.RoadNetwork(roadfile=map_path) # Create entitities diff --git a/tests/core/test_serialization.py b/tests/core/test_serialization.py index e7b88ccc2..c4ca8ca0d 100644 --- a/tests/core/test_serialization.py +++ b/tests/core/test_serialization.py @@ -554,4 +554,4 @@ def test_xosc_export(getMetadriveSimulator): scene, _ = scenario.generate() simulationResult = simulator.simulate(scene) assert simulationResult is not None - xosc_scenario = toOpenScenario(scenario, scene, simulationResult) + xosc_scenario = toOpenScenario(simulationResult, scenario, scene) From ab311ca44bc4bf302ba32ed4b3bb5dc2a16673ee Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 28 May 2026 20:45:31 -0700 Subject: [PATCH 020/134] Minor fixes. --- src/scenic/core/serialization.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 77fb68938..3e339699c 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -429,15 +429,15 @@ def toOpenScenario( xosc_paramdec = xosc.ParameterDeclarations() # Extract map - if map_path is None: + if mapPath is None: if "map" not in scenario.params: raise ValueError( "No `mapPath` provided and scenario does not have a `map` parameter defined." ) - map_path = ( + mapPath = ( mapPath if mapPath is not None else os.path.abspath(scenario.params["map"]) ) - xosc_road = xosc.RoadNetwork(roadfile=map_path) + xosc_road = xosc.RoadNetwork(roadfile=mapPath) # Create entitities entities = xosc.Entities() From 54a2a5eb8b87922ca0e99f953f6e07ed6fe65376 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 29 May 2026 06:46:18 -0700 Subject: [PATCH 021/134] Fixed blank line --- docs/api.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/api.rst b/docs/api.rst index 5cfe99f09..23ac150e7 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -176,6 +176,7 @@ it to the replay, but this greatly increases the size of the encoded simulation. .. _xosc_export: OpenScenarioXML Export ---------------------- + Scenic provides experimental support for exporting completed simulations via `toOpenScenario`. This function currently only supports cars and pedestrians, and may be subject to breaking changes in the future. From 25fe1a108ae56beefba1d2fc5ea039fd627525de Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 29 May 2026 06:56:39 -0700 Subject: [PATCH 022/134] Another blank line? --- docs/api.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/api.rst b/docs/api.rst index 23ac150e7..7f33c12d2 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -174,6 +174,7 @@ it to the replay, but this greatly increases the size of the encoded simulation. compatible across major versions of Scenic. .. _xosc_export: + OpenScenarioXML Export ---------------------- From 518c30810aedbac9a2331b6bbbd3f850cf19cb9a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 29 May 2026 07:09:24 -0700 Subject: [PATCH 023/134] Fix scenariogeneration link --- src/scenic/core/serialization.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 3e339699c..4b0ed9dc7 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -404,7 +404,7 @@ def toOpenScenario( mapPath=None, scenarioName="ScenicScenario", ): - """Export a `SimulationResult` as a `scenariogeneration` `xosc` object. + """Export a `SimulationResult` as a `scenariogeneration.xosc` object. Args: simulationResult: The `SimulationResult` to be exported to XOSC From 96b3ee1bd6419196117a1effe94bb6cc959e4226 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 29 May 2026 08:09:14 -0700 Subject: [PATCH 024/134] Fixed link --- src/scenic/core/serialization.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/serialization.py b/src/scenic/core/serialization.py index 4b0ed9dc7..97c083734 100644 --- a/src/scenic/core/serialization.py +++ b/src/scenic/core/serialization.py @@ -404,7 +404,7 @@ def toOpenScenario( mapPath=None, scenarioName="ScenicScenario", ): - """Export a `SimulationResult` as a `scenariogeneration.xosc` object. + """Export a `SimulationResult` as a `scenariogeneration.xosc.scenario `_ object. Args: simulationResult: The `SimulationResult` to be exported to XOSC From f9557b568a0eb60f626c3faa34d141454cdbbb76 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sat, 30 May 2026 14:15:27 -0700 Subject: [PATCH 025/134] Added support for parallel behaviors. --- src/scenic/core/dynamics/behaviors.py | 37 +++++++++++++++++++++------ src/scenic/core/simulators.py | 2 ++ src/scenic/syntax/compiler.py | 4 +-- tests/syntax/test_dynamics.py | 32 +++++++++++++---------- 4 files changed, 52 insertions(+), 23 deletions(-) diff --git a/src/scenic/core/dynamics/behaviors.py b/src/scenic/core/dynamics/behaviors.py index 7c12ef7c2..ebd85dfc6 100644 --- a/src/scenic/core/dynamics/behaviors.py +++ b/src/scenic/core/dynamics/behaviors.py @@ -109,7 +109,7 @@ def alarmHandler(signum, frame): try: actions = self._runningIterator.send(None) except StopIteration: - actions = () # behavior ended early + return None return actions def _stop(self, reason=None): @@ -124,18 +124,39 @@ def _isFinished(self): def _invokeInner(self, agent, subs): import scenic.syntax.veneer as veneer - assert len(subs) == 1 - sub = subs[0] - if not isinstance(sub, Behavior): - raise TypeError(f"expected a behavior, got {sub}") - sub._start(agent) - with veneer.executeInBehavior(sub): + # Validate all inner behaviors + for sub in subs: + if not isinstance(sub, Behavior): + raise TypeError(f"expected a behavior, got {sub}") + sub._start(agent) + + # Create a generator for each inner behavior that yields the appropriate actions, cleaning up when done. + def make_inner_generator(sub): try: - yield from sub._runningIterator + while sub._isRunning: + actions = sub._step() + if actions is None: + return + yield actions finally: if sub._isRunning: sub._stop() + inner_generators = [make_inner_generator(sub) for sub in subs] + + # Yield from a generator that zips all the inner generators together without padding, until all inner generators have terminated. + while True: + try: + raw_actions = next(itertools.zip_longest(*inner_generators)) + yield tuple( + filter( + lambda x: x is not None, + itertools.chain.from_iterable(raw_actions), + ) + ) + except StopIteration: + return + def __repr__(self): items = itertools.chain( (repr(arg) for arg in self._args), diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index f662c451d..97469f479 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -489,6 +489,8 @@ def _run(self, dynamicScenario, maxSteps): # Run the agent's behavior to get its actions actions = agent.behavior._step() + if actions is None: + actions = tuple() # Handle pseudo-actions marking the end of a simulation/scenario if isinstance(actions, _EndSimulationAction): diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index 0bf027823..ac1beeae9 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1249,9 +1249,9 @@ def visit_TerminateSimulation(self, node: s.TerminateSimulation): @context(Context.DYNAMIC) def visit_Do(self, node: s.Do): - if (self.inBehavior or self.inMonitor) and len(node.elts) > 1: + if self.inMonitor and len(node.elts) > 1: raise self.makeSyntaxError( - f"`do` can only take one action inside a {'behavior' if self.inBehavior else 'monitor'}", + f"`do` can only take one action inside a monitor", node, ) return self.makeDoLike(node, node.elts) diff --git a/tests/syntax/test_dynamics.py b/tests/syntax/test_dynamics.py index 4699a2921..3a7bd3c43 100644 --- a/tests/syntax/test_dynamics.py +++ b/tests/syntax/test_dynamics.py @@ -188,6 +188,25 @@ def test_behavior_take_empty_tuple(): assert tuple(actions) == (None, 7) +def test_parallel_behaviors(): + scenario = compileScenic( + """ + behavior Fizz(): + take "Fizz" + + behavior Buzz(): + take "Buzz" + + behavior Foo(): + take "Start" + do Buzz(), Fizz() + ego = new Object with behavior Foo + """ + ) + actions = sampleEgoActions(scenario, maxSteps=2, singleAction=False) + assert tuple(actions) == (("Start",), ("Buzz", "Fizz")) + + # Various errors @@ -770,19 +789,6 @@ def test_behavior_invoke_mistyped(): sampleActions(scenario) -def test_behavior_invoke_multiple(): - with pytest.raises(ScenicSyntaxError): - compileScenic( - """ - behavior Foo(): - take 5 - behavior Bar(): - do Foo(), Foo() - ego = new Object with behavior Bar - """ - ) - - def test_behavior_tuple_invalid(): scenario = compileScenic( """ From 2c1551d8e560efcf538948603e47b2b85912a48a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sat, 30 May 2026 14:19:24 -0700 Subject: [PATCH 026/134] Naming tweak. --- src/scenic/core/dynamics/behaviors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/dynamics/behaviors.py b/src/scenic/core/dynamics/behaviors.py index ebd85dfc6..f33e2ce8e 100644 --- a/src/scenic/core/dynamics/behaviors.py +++ b/src/scenic/core/dynamics/behaviors.py @@ -147,7 +147,7 @@ def make_inner_generator(sub): # Yield from a generator that zips all the inner generators together without padding, until all inner generators have terminated. while True: try: - raw_actions = next(itertools.zip_longest(*inner_generators)) + raw_actions_list = next(itertools.zip_longest(*inner_generators)) yield tuple( filter( lambda x: x is not None, From 28f7c77eea290e8136a77731353ebde15b56b985 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sat, 30 May 2026 14:22:19 -0700 Subject: [PATCH 027/134] Small fix. --- src/scenic/core/dynamics/behaviors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/dynamics/behaviors.py b/src/scenic/core/dynamics/behaviors.py index f33e2ce8e..1bab40903 100644 --- a/src/scenic/core/dynamics/behaviors.py +++ b/src/scenic/core/dynamics/behaviors.py @@ -151,7 +151,7 @@ def make_inner_generator(sub): yield tuple( filter( lambda x: x is not None, - itertools.chain.from_iterable(raw_actions), + itertools.chain.from_iterable(raw_actions_list), ) ) except StopIteration: From 2e62dc85dc8f32d45658275528231d39ce5139bf Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Sat, 30 May 2026 16:00:57 -0700 Subject: [PATCH 028/134] Consolidate PID controller code --- src/scenic/domains/driving/behaviors.scenic | 13 ++- src/scenic/domains/driving/controllers.py | 108 +++++++------------- 2 files changed, 49 insertions(+), 72 deletions(-) diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 173500190..a7a575cb7 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -39,9 +39,20 @@ behavior WalkForwardBehavior(): behavior ConstantThrottleBehavior(x): take SetThrottleAction(x) +behavior FollowPathBehavior(path, target_speed, controllers): + """ + Follows the given path at the given target_speed, using the specified controller(s). + + Args: + path: A `PolylineRegion` representing the desired vehicle path. + target_speed: The desired speed to maintain. + controllers: Either a tuple (LonController, LatController) or a single controller for + longitudinal and lateral controls. + """ + behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTraffic=False): """ - Follow's the lane on which the vehicle is at, unless the laneToFollow is specified. + Follows the lane on which the vehicle is at, unless the laneToFollow is specified. Once the vehicle reaches an intersection, by default, the vehicle will take the straight route. If straight route is not available, then any availble turn route will be taken, uniformly randomly. If turning at the intersection, the vehicle will slow down to make the turn, safely. diff --git a/src/scenic/domains/driving/controllers.py b/src/scenic/domains/driving/controllers.py index ce7d46135..00c51f3d9 100644 --- a/src/scenic/domains/driving/controllers.py +++ b/src/scenic/domains/driving/controllers.py @@ -12,54 +12,49 @@ .. _CARLA: https://carla.org/ """ +from abc import ABC, abstractmethod from collections import deque import numpy as np -class PIDLongitudinalController: - """Longitudinal control using a PID to reach a target speed. +class LongitudinalController(ABC): + @abstractmethod + def compute_throttle(self): + pass - Arguments: - K_P: Proportional gain - K_D: Derivative gain - K_I: Integral gain - dt: time step - """ - def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1): - self._k_p = K_P - self._k_d = K_D - self._k_i = K_I - self._dt = dt - self._error_buffer = deque(maxlen=10) +class LateralController(ABC): + @abstractmethod + def compute_steering(self): + pass - def run_step(self, speed_error): - """Estimate the throttle/brake of the vehicle based on the PID equations. - Arguments: - speed_error: target speed minus current speed +class PIDController: + def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1): + self.kp = K_P + self.ki = K_I + self.kd = K_D + self.dt = dt + self.i_term = 0 + self.last_error = None + self.windup_guard = 20.0 - Returns: - a signal between -1 and 1, with negative values indicating braking. - """ - error = speed_error - self._error_buffer.append(error) + def run_step(self, error): + # Compute terms + p_term = error + self.i_term += np.clip(error * self.dt, -self.windup_guard, self.windup_guard) + d_term = (error - self.last_error) / self.dt if self.last_error else 0 - if len(self._error_buffer) >= 2: - _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - _ie = sum(self._error_buffer) * self._dt - else: - _de = 0.0 - _ie = 0.0 + # Remember last error for next calculation + self.last_error = error - return np.clip( - (self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0 - ) + output = (self.kp * p_term) + (self.ki * self.i_term) + (self.kd * d_term) + return np.clip(output, -1, 1) -class PIDLateralController: - """Lateral control using a PID to track a trajectory. +class PIDLongitudinalController(PIDController): + """Longitudinal control using a PID to reach a target speed. Arguments: K_P: Proportional gain @@ -68,42 +63,13 @@ class PIDLateralController: dt: time step """ - def __init__(self, K_P=0.3, K_D=0.2, K_I=0, dt=0.1): - self.Kp = K_P - self.Kd = K_D - self.Ki = K_I - self.PTerm = 0 - self.ITerm = 0 - self.DTerm = 0 - self.dt = dt - self.last_error = 0 - self.windup_guard = 20.0 - self.output = 0 - - def run_step(self, cte): - """Estimate the steering angle of the vehicle based on the PID equations. - - Arguments: - cte: cross-track error (distance to right of desired trajectory) - - Returns: - a signal between -1 and 1, with -1 meaning maximum steering to the left. - """ - error = cte - delta_error = error - self.last_error - self.PTerm = self.Kp * error - self.ITerm += error * self.dt - - if self.ITerm < -self.windup_guard: - self.ITerm = -self.windup_guard - elif self.ITerm > self.windup_guard: - self.ITerm = self.windup_guard - - self.DTerm = delta_error / self.dt - - # Remember last error for next calculation - self.last_error = error - self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm) +class PIDLateralController(PIDController): + """Lateral control using a PID to track a trajectory. - return np.clip(self.output, -1, 1) + Arguments: + K_P: Proportional gain + K_D: Derivative gain + K_I: Integral gain + dt: time step + """ From 04003e177facc37c22b39269f6f90d8bc4860b7f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 1 Jun 2026 16:16:20 -0700 Subject: [PATCH 029/134] Initial pure pursuit implementation and reorginization. --- src/scenic/domains/driving/actions.py | 9 +- src/scenic/domains/driving/behaviors.scenic | 197 +++++++++++++------- src/scenic/domains/driving/controllers.py | 114 ++++++++++- src/scenic/domains/driving/model.scenic | 4 +- 4 files changed, 238 insertions(+), 86 deletions(-) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index 7f99cce1a..f2ab406d1 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -15,6 +15,8 @@ import math +import numpy as np + from scenic.core.simulators import Action from scenic.core.vectors import Vector @@ -241,11 +243,8 @@ def __init__( brake = min(abs(throttle), max_brake) # Steering regulation: changes cannot happen abruptly, can't steer too much. - - if steer > past_steer + 0.1: - steer = past_steer + 0.1 - elif steer < past_steer - 0.1: - steer = past_steer - 0.1 + if past_steer is not None: + steer = np.clip(steer, past_steer - 0.1, past_steer + 0.1) if steer >= 0: steer = min(max_steer, steer) diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index a7a575cb7..80b90dd4a 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -49,8 +49,63 @@ behavior FollowPathBehavior(path, target_speed, controllers): controllers: Either a tuple (LonController, LatController) or a single controller for longitudinal and lateral controls. """ + _lon_controller, _lat_controller = controllers + while True: + speed_error = target_speed - self.speed + + # compute throttle : Longitudinal Control + throttle = _lon_controller.run_step(speed_error) + + # compute steering : Lateral Control + last_steer_angle = _lat_controller.lastSteerAngle + current_steer_angle = _lat_controller.computeSteering(path, self) + + print() + take RegulatedControlAction(throttle, current_steer_angle, past_steer=last_steer_angle) + +def getFollowLanePath(obj, minPathDistance, preferStraight, path_metadata=None): + import shapely + import itertools + + def mergeLineStrings(geoms): + return shapely.geometry.LineString(itertools.chain.from_iterable(geom.coords for geom in geoms)) + + if path_metadata is None: + current_lane = ego.lane + initial_path = obj.lane.centerline.lineString + else: + current_lane = path_metadata[0] + initial_path = path_metadata[1] + + assert isinstance(initial_path, shapely.geometry.LineString) + + ego_pt = shapely.geometry.Point(*obj.position) + path = shapely.ops.substring(initial_path, initial_path.project(ego_pt), initial_path.length) + + while path.length < minPathDistance: + straight_manuevers = [m for m in current_lane.maneuvers if m.type == ManeuverType.STRAIGHT] + + if preferStraight and straight_manuevers: + target_maneuver = Uniform(*straight_manuevers) + else: + if len(current_lane.maneuvers) > 0: + target_maneuver = Uniform(*current_lane.maneuvers) + else: + # No more maneuvers. Raise a warning and return centerline as is + warnings.warn("Could not generate path of desired distance. Returning path as is.") + break + + if target_maneuver.connectingLane != None: + path = mergeLineStrings([path, target_maneuver.connectingLane.centerline.lineString, target_maneuver.endLane.centerline.lineString]) + else: + path = mergeLineStrings([path, target_maneuver.endLane.centerline.lineString]) + + current_lane = target_maneuver.endLane -behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTraffic=False): + assert isinstance(path, shapely.geometry.LineString) + return PolylineRegion(polyline=path), (current_lane, path) + +behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, preferStraight=True, controllers=None): """ Follows the lane on which the vehicle is at, unless the laneToFollow is specified. Once the vehicle reaches an intersection, by default, the vehicle will take the straight route. @@ -61,7 +116,9 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra e.g. do FollowLaneBehavior() until ... :param target_speed: Its unit is in m/s. By default, it is set to 10 m/s - :param laneToFollow: If the lane to follow is different from the lane that the vehicle is on, this parameter can be used to specify that lane. By default, this variable will be set to None, which means that the vehicle will follow the lane that it is currently on. + :param laneToFollow: If the lane to follow is different from the lane that the vehicle is on, this + parameter can be used to specify that lane. By default, this variable will be set to None, which + means that the vehicle will follow the lane that it is currently on. """ past_steer_angle = 0 @@ -88,78 +145,71 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, is_oppositeTra nearby_intersection = current_lane.centerline[-1] # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) - - while True: - - if self.speed is not None: - current_speed = self.speed - else: - current_speed = past_speed - - if not entering_intersection and (distance from self.position to nearby_intersection) < TRIGGER_DISTANCE_TO_SLOWDOWN: - entering_intersection = True - intersection_passed = False - straight_manuevers = filter(lambda i: i.type == ManeuverType.STRAIGHT, current_lane.maneuvers) - - if len(straight_manuevers) > 0: - select_maneuver = Uniform(*straight_manuevers) - else: - if len(current_lane.maneuvers) > 0: - select_maneuver = Uniform(*current_lane.maneuvers) - else: - take SetBrakeAction(1.0) - break - - # assumption: there always will be a maneuver - if select_maneuver.connectingLane != None: - current_centerline = concatenateCenterlines([current_centerline, select_maneuver.connectingLane.centerline, \ - select_maneuver.endLane.centerline]) - else: - current_centerline = concatenateCenterlines([current_centerline, select_maneuver.endLane.centerline]) - - current_lane = select_maneuver.endLane - end_lane = current_lane - - if current_lane.maneuvers != (): - nearby_intersection = current_lane.maneuvers[0].intersection - if nearby_intersection == None: - nearby_intersection = current_lane.centerline[-1] - else: - nearby_intersection = current_lane.centerline[-1] - - if select_maneuver.type != ManeuverType.STRAIGHT: - in_turning_lane = True - target_speed = TARGET_SPEED_FOR_TURNING - - do TurnBehavior(trajectory = current_centerline) - - - if (end_lane is not None) and (self.position in end_lane) and not intersection_passed: - intersection_passed = True - in_turning_lane = False - entering_intersection = False - target_speed = original_target_speed - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) - - nearest_line_points = current_centerline.nearestSegmentTo(self.position) - nearest_line_segment = PolylineRegion(nearest_line_points) - cte = nearest_line_segment.signedDistanceTo(self.position) - if is_oppositeTraffic: - cte = -cte - - speed_error = target_speed - current_speed + if controllers: + _lon_controller, _lat_controller = controllers + else: + _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) - # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) + path_metadata = None - # compute steering : Lateral Control - current_steer_angle = _lat_controller.run_step(cte) + # DEBUG + _lat_controller.simulation = simulation() - take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) - past_steer_angle = current_steer_angle - past_speed = current_speed + while True: + # if not entering_intersection and (distance from self.position to nearby_intersection) < TRIGGER_DISTANCE_TO_SLOWDOWN: + # entering_intersection = True + # intersection_passed = False + # straight_manuevers = filter(lambda i: i.type == ManeuverType.STRAIGHT, current_lane.maneuvers) + + # if len(straight_manuevers) > 0: + # select_maneuver = Uniform(*straight_manuevers) + # else: + # if len(current_lane.maneuvers) > 0: + # select_maneuver = Uniform(*current_lane.maneuvers) + # else: + # take SetBrakeAction(1.0) + # break + + # # assumption: there always will be a maneuver + # if select_maneuver.connectingLane != None: + # current_centerline = concatenateCenterlines([current_centerline, select_maneuver.connectingLane.centerline, \ + # select_maneuver.endLane.centerline]) + # else: + # current_centerline = concatenateCenterlines([current_centerline, select_maneuver.endLane.centerline]) + + # current_lane = select_maneuver.endLane + # end_lane = current_lane + + # if current_lane.maneuvers != (): + # nearby_intersection = current_lane.maneuvers[0].intersection + # if nearby_intersection == None: + # nearby_intersection = current_lane.centerline[-1] + # else: + # nearby_intersection = current_lane.centerline[-1] + + # if select_maneuver.type != ManeuverType.STRAIGHT: + # in_turning_lane = True + # target_speed = TARGET_SPEED_FOR_TURNING + + # do TurnBehavior(trajectory=current_centerline, target_speed=target_speed, controllers=(_lon_controller, _lat_controller)) + + + # if (end_lane is not None) and (self.position in end_lane) and not intersection_passed: + # intersection_passed = True + # in_turning_lane = False + # entering_intersection = False + # target_speed = original_target_speed + # # _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) + + # nearest_line_points = current_centerline.nearestSegmentTo(self.position) + # nearest_line_segment = PolylineRegion(nearest_line_points) + # path = nearest_line_segment + + replan_time = 5 + min_path_distance = max(2*replan_time*self.speed, 50) + path, path_metadata = getFollowLanePath(self, min_path_distance, preferStraight=preferStraight, path_metadata=path_metadata) + do FollowPathBehavior(path, target_speed, controllers) for replan_time seconds behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_speed=None): """ @@ -217,7 +267,7 @@ behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_spe -behavior TurnBehavior(trajectory, target_speed=6): +behavior TurnBehavior(trajectory, target_speed=6, controllers=None): """ This behavior uses a controller specifically tuned for turning at an intersection. This behavior is only operational within an intersection, @@ -230,7 +280,10 @@ behavior TurnBehavior(trajectory, target_speed=6): trajectory_centerline = concatenateCenterlines([traj.centerline for traj in trajectory]) # instantiate longitudinal and lateral controllers - _lon_controller, _lat_controller = simulation().getTurningControllers(self) + if controllers: + _lon_controller, _lat_controller = controllers + else: + _lon_controller, _lat_controller = simulation().getTurningControllers(self) past_steer_angle = 0 diff --git a/src/scenic/domains/driving/controllers.py b/src/scenic/domains/driving/controllers.py index 00c51f3d9..5aecf9493 100644 --- a/src/scenic/domains/driving/controllers.py +++ b/src/scenic/domains/driving/controllers.py @@ -14,43 +14,62 @@ from abc import ABC, abstractmethod from collections import deque +import math import numpy as np +import shapely +from shapely.geometry import LineString, MultiPoint, Point as ShapelyPoint + +from scenic.core.regions import CircularRegion, PolylineRegion, toPolygon +from scenic.core.vectors import Vector class LongitudinalController(ABC): @abstractmethod - def compute_throttle(self): + def computeThrottle(self): pass class LateralController(ABC): + def __init__(self): + super().__init__() + self.lastSteerAngle = None + @abstractmethod - def compute_steering(self): + def computeSteering(self, trajectory, obj): pass class PIDController: - def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1): + def __init__(self, K_P, K_D, K_I, dt): + super().__init__() self.kp = K_P self.ki = K_I self.kd = K_D self.dt = dt self.i_term = 0 self.last_error = None - self.windup_guard = 20.0 + self.windup_guard = 0.5 / self.ki if self.ki != 0 else 0 def run_step(self, error): # Compute terms p_term = error - self.i_term += np.clip(error * self.dt, -self.windup_guard, self.windup_guard) + self.i_term += error * self.dt + self.i_term = np.clip(self.i_term, -self.windup_guard, self.windup_guard) d_term = (error - self.last_error) / self.dt if self.last_error else 0 + print(f"Error: {error}, LastError: {self.last_error}") # Remember last error for next calculation self.last_error = error output = (self.kp * p_term) + (self.ki * self.i_term) + (self.kd * d_term) - return np.clip(output, -1, 1) + clipped_output = np.clip(output, -1, 1) + print(f"p_term: {p_term}") + print(f"i_term: {self.i_term}") + print(f"d_term: {d_term}") + print(f"PID Output: {output}") + print(f"PID Clipped Output: {clipped_output}") + return clipped_output class PIDLongitudinalController(PIDController): @@ -63,8 +82,11 @@ class PIDLongitudinalController(PIDController): dt: time step """ + def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1): + super().__init__(K_P, K_D, K_I, dt) + -class PIDLateralController(PIDController): +class PIDLateralController(PIDController, LateralController): """Lateral control using a PID to track a trajectory. Arguments: @@ -73,3 +95,81 @@ class PIDLateralController(PIDController): K_I: Integral gain dt: time step """ + + def __init__(self, K_P=0.3, K_D=0.2, K_I=0, dt=0.1): + super().__init__(K_P, K_D, K_I, dt) + + def computeSteering(self, trajectory, obj): + assert isinstance(trajectory, PolylineRegion) + cte = trajectory.signedDistanceTo(obj.position) + # TODO: opposite traffic check? + steer_angle = self.run_step(cte) + self.lastSteerAngle = steer_angle + return steer_angle + + +class PurePursuitLateralController(LateralController): + def __init__(self, lookaheadDistance=lambda obj: obj.speed + 1): + super().__init__() + self.lookaheadDistance = lookaheadDistance + self._lastTargetPoint = None + + def _findTargetPoint(self, trajectory, obj, lookaheadDistance): + assert isinstance(trajectory, PolylineRegion) + traj_line_string = toPolygon(trajectory) + + # Find candidate target points + obj_pt = ShapelyPoint(*obj.position) + obj_traj_dist = traj_line_string.project(obj_pt) + forward_traj = shapely.ops.substring( + traj_line_string, obj_traj_dist, traj_line_string.length + ) + lookahead_circle = toPolygon( + CircularRegion(forward_traj.coords[0], lookaheadDistance) + ) + intersection_geometry = lookahead_circle.boundary.intersection(forward_traj) + + if intersection_geometry.is_empty: + # No viable target points. If we have a last target point, aim for that. Otherwise, + # aim for the closest point on the trajectory. + if self._lastTargetPoint: + target_point = self._lastTargetPoint + else: + target_point = trajectory.project(obj.position) + elif isinstance(intersection_geometry, ShapelyPoint): + target_point = intersection_geometry + elif isinstance(intersection_geometry, MultiPoint): + # There are multiple candidate target points. Pick the one that appears first on the path. + target_point = sorted( + intersection_geometry.geoms, key=lambda pt: shapely.distance(pt, obj_pt) + )[0] + else: + # We've gotten something strange. Fall back to a representative point as the target. + target_point = intersection_geometry.representative_point() + + # Store last target point and return + self._lastTargetPoint = target_point + return Vector(target_point.x, target_point.y) + + def computeSteering(self, trajectory, obj): + # Compute target steering angle + lookaheadDistance = self.lookaheadDistance(obj) + targetPoint = self._findTargetPoint(trajectory, obj, lookaheadDistance) + alpha = obj.angleTo(targetPoint) - obj.heading + delta = -math.atan2(2 * obj.wheelbase * math.sin(alpha), lookaheadDistance) + + # Convert target steering angle to relative value in [-1, 1] + rel_steering_angle = np.clip(delta / obj.maxSteeringAngle, -1, 1) + + # DEBUG + print(f"SPEED: {obj.speed}") + print(f"ALPHA: {delta}") + print(f"DELTA: {rel_steering_angle}") + print(f"RELATIVE STEERING ANGLE: {rel_steering_angle}") + # import pygame + # pygame.draw.circle(self.simulation.screen, (0,1,0), self.simulation.scenicToScreenVal(targetPoint), 5) + # pygame.display.update() + # import time + # time.sleep(0.2) + self.lastSteerAngle = rel_steering_angle + return rel_steering_angle diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index 8db0c2805..f5fbed5ae 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -290,7 +290,7 @@ class Vehicle(DrivingObject): times the length of the vehicle. maxSteeringAngle: The maximum steering angle of the vehicle. The full steering range would be two times this value, going from (-maxSteeringAngle, maxSteeringAngle). Default value - 30 degrees. + 40 degrees. wheelDiameter: The diameter of the *entire* wheel (including the tire). Default value is 0.7 meters. trackWidth: Distance between the vehicle's wheels when pointed straight ahead. Default value is 0.85 times the width of the vehicle. @@ -314,7 +314,7 @@ class Vehicle(DrivingObject): length: 4.5 color: Color.defaultCarColor() wheelbase: 0.6*self.length - maxSteeringAngle: 35 deg + maxSteeringAngle: 40 deg wheelDiameter: 0.7 trackWidth: 0.85*self.width groundClearance: 0.5*self.wheelDiameter From 6d802b231f4aaf0fda2acb1f3789b9f21efa2c2e Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 22 Jun 2026 16:59:27 -0700 Subject: [PATCH 030/134] Progress on controllers --- src/scenic/domains/driving/behaviors.scenic | 201 +++++++++----------- src/scenic/domains/driving/controllers.py | 99 +++++----- src/scenic/domains/driving/model.scenic | 12 ++ 3 files changed, 147 insertions(+), 165 deletions(-) diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 80b90dd4a..1ca88063a 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -4,13 +4,16 @@ These behaviors are automatically imported when using the driving domain. """ import math +from abc import ABC, abstractmethod +import shapely +from shapely.geometry import LineString, MultiPoint, Point as ShapelyPoint + +from scenic.core.regions import toPolygon from scenic.domains.driving.actions import * import scenic.domains.driving.model as _model from scenic.domains.driving.roads import ManeuverType -def concatenateCenterlines(centerlines=[]): - return PolylineRegion.unionAll(centerlines) behavior ConstantThrottleBehavior(x): while True: @@ -39,30 +42,6 @@ behavior WalkForwardBehavior(): behavior ConstantThrottleBehavior(x): take SetThrottleAction(x) -behavior FollowPathBehavior(path, target_speed, controllers): - """ - Follows the given path at the given target_speed, using the specified controller(s). - - Args: - path: A `PolylineRegion` representing the desired vehicle path. - target_speed: The desired speed to maintain. - controllers: Either a tuple (LonController, LatController) or a single controller for - longitudinal and lateral controls. - """ - _lon_controller, _lat_controller = controllers - while True: - speed_error = target_speed - self.speed - - # compute throttle : Longitudinal Control - throttle = _lon_controller.run_step(speed_error) - - # compute steering : Lateral Control - last_steer_angle = _lat_controller.lastSteerAngle - current_steer_angle = _lat_controller.computeSteering(path, self) - - print() - take RegulatedControlAction(throttle, current_steer_angle, past_steer=last_steer_angle) - def getFollowLanePath(obj, minPathDistance, preferStraight, path_metadata=None): import shapely import itertools @@ -105,7 +84,7 @@ def getFollowLanePath(obj, minPathDistance, preferStraight, path_metadata=None): assert isinstance(path, shapely.geometry.LineString) return PolylineRegion(polyline=path), (current_lane, path) -behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, preferStraight=True, controllers=None): +behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, preferStraight=True): """ Follows the lane on which the vehicle is at, unless the laneToFollow is specified. Once the vehicle reaches an intersection, by default, the vehicle will take the straight route. @@ -121,97 +100,97 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, preferStraight means that the vehicle will follow the lane that it is currently on. """ - past_steer_angle = 0 - past_speed = 0 # making an assumption here that the agent starts from zero speed - if laneToFollow is None: - current_lane = self.lane - else: - current_lane = laneToFollow + assert self.longitudinalController is not None + assert self.lateralController is not None - current_centerline = current_lane.centerline - in_turning_lane = False # assumption that the agent is not instantiated within a connecting lane - intersection_passed = False - entering_intersection = False # assumption that the agent is not instantiated within an intersection - end_lane = None - original_target_speed = target_speed - TARGET_SPEED_FOR_TURNING = 5 # KM/H - TRIGGER_DISTANCE_TO_SLOWDOWN = 10 # FOR TURNING AT INTERSECTIONS + path_metadata = None - if current_lane.maneuvers != (): - nearby_intersection = current_lane.maneuvers[0].intersection - if nearby_intersection == None: - nearby_intersection = current_lane.centerline[-1] - else: - nearby_intersection = current_lane.centerline[-1] + while True: + replan_time = 10 + min_path_distance = max(2*replan_time*target_speed, 50) + path, path_metadata = getFollowLanePath(self, min_path_distance, preferStraight=preferStraight, path_metadata=path_metadata) + traj = Trajectory.createFixedSpeedTrajectory(path, target_speed, ts=simulation().timestep) + do FollowTrajectoryBehavior(traj) for replan_time seconds - # instantiate longitudinal and lateral controllers - if controllers: - _lon_controller, _lat_controller = controllers - else: - _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) +class Trajectory(object): + def __init__(self, polyline, ts): + assert isinstance(polyline, PolylineRegion) - path_metadata = None + self.polyline = polyline + self.ts = ts - # DEBUG - _lat_controller.simulation = simulation() + @property + def start(self): + return self.polyline.start - while True: + @property + def end(self): + return self.polyline.end - # if not entering_intersection and (distance from self.position to nearby_intersection) < TRIGGER_DISTANCE_TO_SLOWDOWN: - # entering_intersection = True - # intersection_passed = False - # straight_manuevers = filter(lambda i: i.type == ManeuverType.STRAIGHT, current_lane.maneuvers) - - # if len(straight_manuevers) > 0: - # select_maneuver = Uniform(*straight_manuevers) - # else: - # if len(current_lane.maneuvers) > 0: - # select_maneuver = Uniform(*current_lane.maneuvers) - # else: - # take SetBrakeAction(1.0) - # break - - # # assumption: there always will be a maneuver - # if select_maneuver.connectingLane != None: - # current_centerline = concatenateCenterlines([current_centerline, select_maneuver.connectingLane.centerline, \ - # select_maneuver.endLane.centerline]) - # else: - # current_centerline = concatenateCenterlines([current_centerline, select_maneuver.endLane.centerline]) - - # current_lane = select_maneuver.endLane - # end_lane = current_lane - - # if current_lane.maneuvers != (): - # nearby_intersection = current_lane.maneuvers[0].intersection - # if nearby_intersection == None: - # nearby_intersection = current_lane.centerline[-1] - # else: - # nearby_intersection = current_lane.centerline[-1] - - # if select_maneuver.type != ManeuverType.STRAIGHT: - # in_turning_lane = True - # target_speed = TARGET_SPEED_FOR_TURNING - - # do TurnBehavior(trajectory=current_centerline, target_speed=target_speed, controllers=(_lon_controller, _lat_controller)) - - - # if (end_lane is not None) and (self.position in end_lane) and not intersection_passed: - # intersection_passed = True - # in_turning_lane = False - # entering_intersection = False - # target_speed = original_target_speed - # # _lon_controller, _lat_controller = simulation().getLaneFollowingControllers(self) - - # nearest_line_points = current_centerline.nearestSegmentTo(self.position) - # nearest_line_segment = PolylineRegion(nearest_line_points) - # path = nearest_line_segment - - replan_time = 5 - min_path_distance = max(2*replan_time*self.speed, 50) - path, path_metadata = getFollowLanePath(self, min_path_distance, preferStraight=preferStraight, path_metadata=path_metadata) - do FollowPathBehavior(path, target_speed, controllers) for replan_time seconds + @property + def duration(self): + return self.ts * len(self.polyline.points) + + @property + def length(self): + return self.polyline.length + + def getRelativeTime(self, pos): + return toPolygon(self.polyline).project(ShapelyPoint(*pos), normalized=True)*self.duration + + def getTimedDistance(self, timeA, timeB): + return shapely.ops.substring(toPolygon(self.polyline), timeA/self.duration, timeB/self.duration, normalized=True).length + + def __getitem__(self, time): + pt = toPolygon(self.polyline).interpolate(time/self.duration, normalized=True) + return Vector(pt.x, pt.y) + + @staticmethod + def createFixedSpeedTrajectory(polyline, targetSpeed, ts): + target_dist = 0 + points = [] + while target_dist < polyline.length: + points.append(polyline.lineString.interpolate(target_dist)) + target_dist += targetSpeed*ts + + return Trajectory(PolylineRegion(polyline=LineString(points)), ts=ts) + +behavior FollowTrajectoryBehavior(trajectory, terminationDistance=1): + """ + Follows the given `Trajectory`. + + The behavior terminates when either of the following conditions are met the vehicle position is within + `terminationDistance` of the end of the trajectory. -behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_speed=None): + Args: + trajectory: A `Trajectory`. + terminationDistance: The behavior will terminate when the vehicle position is within `terminationDistance` + of the end of the trajectory. + """ + assert isinstance(trajectory, Trajectory) + assert self.longitudinalController is not None + assert self.lateralController is not None + + while distance from self.position to trajectory.end > terminationDistance: + # Compute throttle : Longitudinal Control + throttle = self.longitudinalController.computeThrottle(trajectory, self) + if throttle > 0: + throttle_action = SetThrottleAction(throttle) + else: + throttle_action = SetBrakeAction(-throttle) + + # Compute steering : Lateral Control + steer = self.lateralController.computeSteering(trajectory, self, simulation()) + steer_action = SetSteerAction(steer) + + take throttle_action, steer_action + +## Legacy Behaviors ## + +def concatenateCenterlines(centerlines=[]): + return PolylineRegion.unionAll(centerlines) + +behavior FollowTrajectoryBehaviorOld(target_speed = 10, trajectory = None, turn_speed=None): """ Follows the given trajectory. The behavior terminates once the end of the trajectory is reached. @@ -265,8 +244,6 @@ behavior FollowTrajectoryBehavior(target_speed = 10, trajectory = None, turn_spe take RegulatedControlAction(throttle, current_steer_angle, past_steer_angle) past_steer_angle = current_steer_angle - - behavior TurnBehavior(trajectory, target_speed=6, controllers=None): """ This behavior uses a controller specifically tuned for turning at an intersection. diff --git a/src/scenic/domains/driving/controllers.py b/src/scenic/domains/driving/controllers.py index 5aecf9493..59ce92868 100644 --- a/src/scenic/domains/driving/controllers.py +++ b/src/scenic/domains/driving/controllers.py @@ -26,27 +26,23 @@ class LongitudinalController(ABC): @abstractmethod - def computeThrottle(self): + def computeThrottle(self, trajectory, veh): pass class LateralController(ABC): - def __init__(self): - super().__init__() - self.lastSteerAngle = None - @abstractmethod - def computeSteering(self, trajectory, obj): + def computeSteering(self, trajectory, veh): pass class PIDController: - def __init__(self, K_P, K_D, K_I, dt): + def __init__(self, dt, *, K_P, K_D, K_I, wg): super().__init__() + self.dt = dt self.kp = K_P self.ki = K_I self.kd = K_D - self.dt = dt self.i_term = 0 self.last_error = None self.windup_guard = 0.5 / self.ki if self.ki != 0 else 0 @@ -58,71 +54,73 @@ def run_step(self, error): self.i_term = np.clip(self.i_term, -self.windup_guard, self.windup_guard) d_term = (error - self.last_error) / self.dt if self.last_error else 0 - print(f"Error: {error}, LastError: {self.last_error}") # Remember last error for next calculation self.last_error = error output = (self.kp * p_term) + (self.ki * self.i_term) + (self.kd * d_term) clipped_output = np.clip(output, -1, 1) - print(f"p_term: {p_term}") - print(f"i_term: {self.i_term}") - print(f"d_term: {d_term}") - print(f"PID Output: {output}") - print(f"PID Clipped Output: {clipped_output}") return clipped_output -class PIDLongitudinalController(PIDController): +class PIDLongitudinalController(PIDController, LongitudinalController): """Longitudinal control using a PID to reach a target speed. Arguments: + dt: time step K_P: Proportional gain K_D: Derivative gain K_I: Integral gain - dt: time step + wg: The windup guard's cap on the integral components contribution + to the total control signal. """ - def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1): - super().__init__(K_P, K_D, K_I, dt) + def __init__(self, dt=0.1, *, K_P=0.5, K_D=0.1, K_I=0.2, wg=0.5): + super().__init__(dt=dt, K_P=K_P, K_D=K_D, K_I=K_I, wg=wg) + + def computeThrottle(self, trajectory, veh): + curr_time = trajectory.getRelativeTime(veh.position) + ts_dist = trajectory.getTimedDistance(curr_time, curr_time + trajectory.ts) + target_speed = ts_dist / trajectory.ts + + cte = target_speed - veh.speed + return self.run_step(cte) class PIDLateralController(PIDController, LateralController): """Lateral control using a PID to track a trajectory. Arguments: + dt: time step K_P: Proportional gain K_D: Derivative gain K_I: Integral gain - dt: time step + wg: The windup guard's cap on the integral components contribution + to the total control signal. """ - def __init__(self, K_P=0.3, K_D=0.2, K_I=0, dt=0.1): - super().__init__(K_P, K_D, K_I, dt) + def __init__(self, dt=0.1, *, K_P=0.3, K_D=0.2, K_I=0, wg=0): + super().__init__(dt=dt, K_P=K_P, K_D=K_D, K_I=K_I, wg=wg) - def computeSteering(self, trajectory, obj): - assert isinstance(trajectory, PolylineRegion) - cte = trajectory.signedDistanceTo(obj.position) - # TODO: opposite traffic check? + def computeSteering(self, trajectory, veh): + cte = trajectory.signedDistanceTo(veh.position) steer_angle = self.run_step(cte) - self.lastSteerAngle = steer_angle return steer_angle class PurePursuitLateralController(LateralController): - def __init__(self, lookaheadDistance=lambda obj: obj.speed + 1): + def __init__(self, lookaheadDistance=lambda veh: veh.speed + 1): super().__init__() self.lookaheadDistance = lookaheadDistance self._lastTargetPoint = None - def _findTargetPoint(self, trajectory, obj, lookaheadDistance): - assert isinstance(trajectory, PolylineRegion) - traj_line_string = toPolygon(trajectory) + def _findTargetPoint(self, trajectory, veh, lookaheadDistance): + traj_line_string = toPolygon(trajectory.polyline) # Find candidate target points - obj_pt = ShapelyPoint(*obj.position) - obj_traj_dist = traj_line_string.project(obj_pt) + veh_pt = ShapelyPoint(*veh.position) + veh_traj_dist = traj_line_string.project(veh_pt) forward_traj = shapely.ops.substring( - traj_line_string, obj_traj_dist, traj_line_string.length + traj_line_string, veh_traj_dist, traj_line_string.length ) lookahead_circle = toPolygon( CircularRegion(forward_traj.coords[0], lookaheadDistance) @@ -135,13 +133,16 @@ def _findTargetPoint(self, trajectory, obj, lookaheadDistance): if self._lastTargetPoint: target_point = self._lastTargetPoint else: - target_point = trajectory.project(obj.position) + target_point = trajectory.polyline.project(veh.position) + + # Our target point isn't actually at the correct lookaheadDistance, so we need to update it. + lookaheadDistance = shapely.distance(veh_pt, target_point) elif isinstance(intersection_geometry, ShapelyPoint): target_point = intersection_geometry elif isinstance(intersection_geometry, MultiPoint): # There are multiple candidate target points. Pick the one that appears first on the path. target_point = sorted( - intersection_geometry.geoms, key=lambda pt: shapely.distance(pt, obj_pt) + intersection_geometry.geoms, key=lambda pt: traj_line_string.project(pt) )[0] else: # We've gotten something strange. Fall back to a representative point as the target. @@ -151,25 +152,17 @@ def _findTargetPoint(self, trajectory, obj, lookaheadDistance): self._lastTargetPoint = target_point return Vector(target_point.x, target_point.y) - def computeSteering(self, trajectory, obj): + def computeSteering(self, trajectory, veh, simulation): # Compute target steering angle - lookaheadDistance = self.lookaheadDistance(obj) - targetPoint = self._findTargetPoint(trajectory, obj, lookaheadDistance) - alpha = obj.angleTo(targetPoint) - obj.heading - delta = -math.atan2(2 * obj.wheelbase * math.sin(alpha), lookaheadDistance) + lookaheadDistance = self.lookaheadDistance(veh) + targetPoint = self._findTargetPoint(trajectory, veh, lookaheadDistance) + rw_position = veh.position.offsetRotated( + veh.heading, Vector(0, -0.5 * veh.wheelbase, 0) + ) + alpha = rw_position.angleTo(targetPoint) - veh.heading + delta = -math.atan2(2 * veh.wheelbase * math.sin(alpha), lookaheadDistance) # Convert target steering angle to relative value in [-1, 1] - rel_steering_angle = np.clip(delta / obj.maxSteeringAngle, -1, 1) - - # DEBUG - print(f"SPEED: {obj.speed}") - print(f"ALPHA: {delta}") - print(f"DELTA: {rel_steering_angle}") - print(f"RELATIVE STEERING ANGLE: {rel_steering_angle}") - # import pygame - # pygame.draw.circle(self.simulation.screen, (0,1,0), self.simulation.scenicToScreenVal(targetPoint), 5) - # pygame.display.update() - # import time - # time.sleep(0.2) - self.lastSteerAngle = rel_steering_angle + rel_steering_angle = np.clip(delta / veh.maxSteeringAngle, -1, 1) + return rel_steering_angle diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index f5fbed5ae..b13298c51 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -313,6 +313,10 @@ class Vehicle(DrivingObject): width: 2 length: 4.5 color: Color.defaultCarColor() + + lateralController: None + longitudinalController: None + wheelbase: 0.6*self.length maxSteeringAngle: 40 deg wheelDiameter: 0.7 @@ -326,6 +330,14 @@ class Vehicle(DrivingObject): def isVehicle(self): return True + def startDynamicSimulation(self): + defaultLongitudinalController, defaultLateralController = simulation().getLaneFollowingControllers(self) + + if self.longitudinalController is None: + self.longitudinalController = defaultLongitudinalController + if self.lateralController is None: + self.lateralController = defaultLateralController + class Car(Vehicle): """A car.""" @property From 5678af3b508f5d73b916ae630a2fcb7eb0fce6a0 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 25 Jun 2026 16:11:30 -0700 Subject: [PATCH 031/134] Added minimum distance operator. (pending final name) --- docs/reference/operators.rst | 8 ++++++ src/scenic/core/object_types.py | 14 +++++++++ src/scenic/core/regions.py | 16 +++++++++++ src/scenic/syntax/ast.py | 6 ++++ src/scenic/syntax/compiler.py | 11 +++++++ src/scenic/syntax/scenic.gram | 7 +++++ src/scenic/syntax/veneer.py | 13 +++++++++ tests/syntax/test_operators.py | 51 +++++++++++++++++++++++++++++++++ 8 files changed, 126 insertions(+) diff --git a/docs/reference/operators.rst b/docs/reference/operators.rst index 4a39539e9..845757793 100644 --- a/docs/reference/operators.rst +++ b/docs/reference/operators.rst @@ -35,6 +35,14 @@ distance [from *vector*] to *vector* ------------------------------------- The distance to the given position from ego (or the position provided with the optional from vector) +.. _minimum distance [from {Object}] to {Object}: +.. _minimum distance from: + +minimum distance [from *Object*] to *Object* +-------------------------------------------- +The minimum distance to the given Object from ego (or the Object provided with the optional from Object). Unlike :ref:`distance from`, this operator takes into account the Objects' shapes, sizes, etc... + + .. _angle [from {vector}] to {vector}: angle [from *vector* ] to *vector* diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 11ff01004..269b4b089 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -1168,6 +1168,20 @@ def distanceTo(self, point): """The minimal distance from the space this object occupies to a given point""" return self.occupiedSpace.distanceTo(point) + @cached_method + def minimumDistanceTo(self, other): + """The minimal distance between this object and another.""" + if not isinstance(other, Object): + raise RuntimeError( + f"Cannot compute minimum distance between Object and {type(other)} " + ) + + # 2D fast path + if self._isPlanarBox and other._isPlanarBox and self.z == other.z: + return self._boundingPolygon.distance(other._boundingPolygon) + + return self.occupiedSpace.minimumDistanceTo(other.occupiedSpace) + @cached_method def intersects(self, other): """Whether or not this object intersects another object or region""" diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 13afc1348..c7a4122f6 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -1759,6 +1759,22 @@ def distanceTo(self, point): return abs(dist) + @distributionFunction + def minimumDistanceTo(self, other): + """Get the minimum distance between this region and another. + + Currently only supports other as a `MeshVolumeRegion`, and is + primarily used for computing minimum distance between objects. + """ + if not isinstance(other, MeshVolumeRegion): + raise NotImplementedError( + f"Cannot compute distance between MeshVolumeRegion and {type(other)}" + ) + + selfObj = fcl.CollisionObject(*self._fclData) + otherObj = fcl.CollisionObject(*other._fclData) + return fcl.distance(selfObj, otherObj) + @cached_property @distributionFunction def inradius(self): diff --git a/src/scenic/syntax/ast.py b/src/scenic/syntax/ast.py index f21a49aaf..a70cba1ae 100644 --- a/src/scenic/syntax/ast.py +++ b/src/scenic/syntax/ast.py @@ -446,6 +446,12 @@ class DistanceFromOp(AST): base: Optional[ast.AST] = None +class MinDistanceFromOp(AST): + # because `to` and `from` are symmetric, the first operand will be `target` and the second will be `base` + target: ast.AST + base: Optional[ast.AST] = None + + class DistancePastOp(AST): target: ast.AST base: Optional[ast.AST] = None diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index 0bf027823..c32dffd00 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1705,6 +1705,17 @@ def visit_DistanceFromOp(self, node: s.DistanceFromOp): ), ) + def visit_MinDistanceFromOp(self, node: s.MinDistanceFromOp): + return ast.Call( + func=ast.Name(id="MinDistanceFrom", ctx=loadCtx), + args=[self.visit(node.target)], + keywords=( + [ast.keyword(arg="Y", value=self.visit(node.base))] + if node.base is not None + else [] + ), + ) + def visit_DistancePastOp(self, node: s.DistancePastOp): return ast.Call( func=ast.Name(id="DistancePast", ctx=loadCtx), diff --git a/src/scenic/syntax/scenic.gram b/src/scenic/syntax/scenic.gram index ec3e63f10..0f87f0a21 100644 --- a/src/scenic/syntax/scenic.gram +++ b/src/scenic/syntax/scenic.gram @@ -1853,6 +1853,8 @@ scenic_prefix_operators: # distance past | "distance" "past" e1=expression 'of' e2=scenic_prefix_operators { s.DistancePastOp(target=e1, base=e2, LOCATIONS) } | "distance" "past" e1=scenic_prefix_operators { s.DistancePastOp(target=e1, LOCATIONS) } + # minimum distance from/to + | &"minimum" scenic_min_distance_from_op # angle from/to | &"angle" scenic_angle_from_op # altitude from/to @@ -1868,6 +1870,11 @@ scenic_distance_from_op: | "distance" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.DistanceFromOp(target=e1, base=e2, LOCATIONS) } | "distance" ('to'|'from') e1=scenic_prefix_operators { s.DistanceFromOp(target=e1, LOCATIONS) } +scenic_min_distance_from_op: + | "minimum" "distance" 'from' e1=expression 'to' e2=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, base=e2, LOCATIONS) } + | "minimum" "distance" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, base=e2, LOCATIONS) } + | "minimum" "distance" ('to'|'from') e1=scenic_prefix_operators { s.MinDistanceFromOp(target=e1, LOCATIONS) } + scenic_angle_from_op: | "angle" 'from' e1=expression 'to' e2=scenic_prefix_operators { s.AngleFromOp(base=e1, target=e2, LOCATIONS) } | "angle" 'to' e1=expression 'from' e2=scenic_prefix_operators { s.AngleFromOp(target=e1, base=e2, LOCATIONS) } diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index b79745030..96cd10acc 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -68,6 +68,7 @@ "ApparentHeading", "RelativePosition", "DistanceFrom", + "MinDistanceFrom", "DistancePast", "Follow", "AngleTo", @@ -1310,6 +1311,18 @@ def DistanceFrom(X, Y=None): return X.distanceTo(Y) +def MinDistanceFrom(X, Y=None): + """The :grammar:`minimum distance from [to ]` operator. + + If the :grammar:`to ` is omitted, the ego is used. + """ + X = toTypes(X, (Object,), '"minimum distance from X to Y" with X not an Object') + if Y is None: + Y = ego() + Y = toTypes(Y, (Object,), '"minimum distance from X to Y" with Y not an Object') + return X.minimumDistanceTo(Y) + + def DistancePast(X, Y=None): """The :grammar:`distance past of ` operator. diff --git a/tests/syntax/test_operators.py b/tests/syntax/test_operators.py index 7c405c789..b74e1c5c6 100644 --- a/tests/syntax/test_operators.py +++ b/tests/syntax/test_operators.py @@ -188,6 +188,57 @@ def test_distance_to_region(): assert p == pytest.approx(2) +# Minimum Distance +def test_minimum_distance(): + p = sampleParamPFrom( + """ + ego = new Object at (1.5, 2, 2.5), + with width 1, with length 2, with height 3 + other = new Object at (-10, -10, -10), + with width 2, with length 2, with height 2 + param p = minimum distance to other + """ + ) + assert p == pytest.approx(math.hypot(10, 10, 10)) + + +def test_minimum_distance_from(): + p = sampleParamPFrom( + """ + foo = new Object at (1.5, 2, 2.5), + with width 1, with length 2, with height 3 + bar = new Object at (-10, -10, -10), + with width 2, with length 2, with height 2 + param p = minimum distance from foo to bar + """ + ) + assert p == pytest.approx(math.hypot(10, 10, 10)) + + +def test_minimum_distance_no_ego(): + with pytest.raises(InvalidScenarioError): + sampleParamPFrom( + """ + other = new Object at (-10, -10, -10), + with width 2, with length 2, with height 2 + param p = minimum distance to other + """ + ) + + +def test_minimum_distance_2d(): + p = sampleParamPFrom( + """ + ego = new Object at (1.5, 2), + with width 1, with length 2 + other = new Object at (-10, -10), + with width 2, with length 2 + param p = minimum distance to other + """ + ) + assert p == pytest.approx(math.hypot(10, 10)) + + # Distance past From 12f60bbc12249356abe2409eabef75e3dba3d169 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Thu, 25 Jun 2026 22:52:49 -0700 Subject: [PATCH 032/134] fix `terminate when` and `record` statements in subscenarios --- docs/reference/dynamic_scenarios.rst | 7 ++- docs/reference/statements.rst | 2 +- src/scenic/core/dynamics/scenarios.py | 62 +++++++++++++++++---------- src/scenic/core/requirements.py | 5 +++ src/scenic/core/sensors.py | 10 +++-- src/scenic/core/simulators.py | 38 ++++++---------- src/scenic/syntax/veneer.py | 62 +++++++++------------------ tests/syntax/test_modular.py | 60 ++++++++++++++++++++++++-- 8 files changed, 143 insertions(+), 103 deletions(-) diff --git a/docs/reference/dynamic_scenarios.rst b/docs/reference/dynamic_scenarios.rst index ad4512912..524570350 100644 --- a/docs/reference/dynamic_scenarios.rst +++ b/docs/reference/dynamic_scenarios.rst @@ -33,12 +33,12 @@ In detail, a single time step of a dynamic simulation is executed according to t If the block executes a :keyword:`require` statement with a false condition, reject the simulation. If it executes :keyword:`terminate` or :keyword:`terminate simulation`, or finishes executing, go to step (e) below to stop the scenario. - e. If the scenario is stopping for one of the reasons above, first recursively stop any sub-scenarios it is running, then revert the effects of any :keyword:`override` statements it executed. + e. If the scenario is stopping for one of the reasons above, save the values of any :keyword:`record final` statements in the scenario, recursively stop any sub-scenarios it is running, then revert the effects of any :keyword:`override` statements it executed. Next, check if any of its :term:`temporal requirements` were not satisfied: if so, reject the simulation. Otherwise, the scenario returns to its parent scenario if it was invoked using :keyword:`do`; if it was the top-level scenario, or if it executed :keyword:`terminate simulation`, we set a flag indicating the top-level scenario has terminated. (We do not terminate immediately since we still need to check monitors in the next step.) -2. Save the values of all :keyword:`record` statements, as well as :keyword:`record initial` statements if it is time step 0. +2. Save the values of all :keyword:`record` statements in currently-running scenarios, as well as :keyword:`record initial` statements for scenarios which have just started. 3. Run each :term:`monitor` instantiated in the currently-running scenarios for one time step (i.e. resume it until it executes :keyword:`wait`). If it executes a :keyword:`require` statement with a false condition, reject the simulation. @@ -66,8 +66,7 @@ In detail, a single time step of a dynamic simulation is executed according to t 9. Update every :term:`dynamic property` of every object to its current value in the simulator. -10. If the simulation is stopping for one of the reasons above, first check if any of the :term:`temporal requirements` of any remaining scenarios were not satisfied: if so, reject the simulation. - Otherwise, save the values of any :keyword:`record final` statements. +10. If the simulation is stopping for one of the reasons above, stop any remaining scenarios as in step (1e) above (including checking :term:`temporal requirements` and saving the values of :keyword:`record final` statements). .. rubric:: Footnotes diff --git a/docs/reference/statements.rst b/docs/reference/statements.rst index aa1de02c0..97a8e5c02 100644 --- a/docs/reference/statements.rst +++ b/docs/reference/statements.rst @@ -285,7 +285,7 @@ The default mutation system adds Gaussian noise to the :prop:`position` and :pro record [initial | final] *value* [as *name*] ---------------------------------------------- Record the value of an expression during each simulation. -The value can be recorded at the start of the simulation (``initial``), at the end of the simulation (``final``), or at every time step (if neither ``initial`` nor ``final`` is specified). +The value can be recorded at the start of the scenario (``initial``), at the end of the scenario (``final``), or at every time step during the scenario (if neither ``initial`` nor ``final`` is specified). The recorded values are available in the ``records`` dictionary of `SimulationResult`: its keys are the given names of the records (or synthesized names if not provided), and the corresponding values are either the value of the recorded expression or a tuple giving its value at each time step as appropriate. For debugging, the records can also be printed out using the :option:`--show-records` command-line option. diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index 8aa1021b5..d931e9587 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -88,6 +88,7 @@ def __init__(self, *args, **kwargs): self._timeLimitInSteps = None # computed at simulation time self._elapsedTime = 0 + self._recordedTime = None self._eventuallySatisfied = None self._overrides = {} @@ -215,10 +216,13 @@ def _start(self): # Prepare recorders simName = veneer.currentSimulation.name + currentTime = veneer.currentSimulation.currentTime globalParams = types.MappingProxyType(veneer._globalParameters) for req in self._recordedExprs: if (recConfig := req.recConfig) and (recorder := recConfig.recorder): - recorder.beginRecording(recConfig, simName, timestep, globalParams) + recorder.beginRecording( + recConfig, simName, timestep, globalParams, currentTime + ) def _step(self): """Execute the (already-started) scenario for one time step. @@ -294,6 +298,15 @@ def _stop(self, reason, quiet=False): assert self._isRunning + if not quiet: + # Record finally-recorded values. + sim = veneer.currentSimulation + for rec in self._recordedFinalExprs: + sim._record(rec.name, rec.evaluate()) + + # Record ordinary `record` statements too if they haven't been already. + self._recordTimeSeries() + # Stop monitors and subscenarios. for monitor in self._monitors: if monitor._isRunning: @@ -356,28 +369,33 @@ def _invokeInner(self, agent, subs): # Check if any sub-scenarios stopped during action execution self._subScenarios = [sub for sub in self._subScenarios if sub._isRunning] - def _evaluateRecordedExprs(self, ty, step): - if ty is RequirementType.record: - place = "_recordedExprs" - elif ty is RequirementType.recordInitial: - place = "_recordedInitialExprs" - elif ty is RequirementType.recordFinal: - place = "_recordedFinalExprs" - else: - assert False, "invalid record type requested" - return self._evaluateRecordedExprsAt(place, step) - - def _evaluateRecordedExprsAt(self, place, step): - values = {} - for rec in getattr(self, place): - value = rec.evaluate() - values[rec.name] = value - if (recConfig := rec.recConfig) and (recorder := recConfig.recorder): - recorder._record(value, step) + def _updateRecords(self): + from scenic.syntax.veneer import currentSimulation + + # _step() was called earlier this time step, so at time step 0 we will + # already have _elapsedTime == 1 + assert self._elapsedTime >= 1 + if self._elapsedTime == 1: + for rec in self._recordedInitialExprs: + currentSimulation._record(rec.name, rec.evaluate()) + + self._recordTimeSeries() + for sub in self._subScenarios: - subvals = sub._evaluateRecordedExprsAt(place, step) - values.update(subvals) - return values + sub._updateRecords() + + def _recordTimeSeries(self): + from scenic.syntax.veneer import currentSimulation + + if self._recordedTime == currentSimulation.currentTime: + # This time step was already recorded (e.g. the scenario was terminated + # by a behavior after the current state was recorded). + return + + for rec in self._recordedExprs: + currentSimulation._recordTimeSeries(rec.name, rec.evaluate()) + + self._recordedTime = currentSimulation.currentTime def _runMonitors(self): terminationReason = None diff --git a/src/scenic/core/requirements.py b/src/scenic/core/requirements.py index 9c29b12e8..f816d429b 100644 --- a/src/scenic/core/requirements.py +++ b/src/scenic/core/requirements.py @@ -15,6 +15,7 @@ from scenic.core.errors import InvalidScenarioError from scenic.core.lazy_eval import needsLazyEvaluation from scenic.core.propositions import Atomic, PropositionNode +from scenic.core.utils import DefaultIdentityDict import scenic.syntax.relations as relations @@ -458,6 +459,10 @@ def falsifiedByInner(self, sample): one_time_monitor = self.proposition.create_monitor() return self.closure(sample, one_time_monitor) == rv_ltl.B4.FALSE + def evaluate(self): + # Used only for `terminate when`, etc. defined in setup blocks of subscenarios + return self.closure(DefaultIdentityDict()) + def __str__(self): if self.name: return self.name diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index db15f27df..c4805ea10 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -101,7 +101,7 @@ class Recorder: def __init__(self): self._recording = False - def beginRecording(self, config, simulationName, timestep, globalParams): + def beginRecording(self, config, simulationName, timestep, globalParams, currentTime): assert not self._recording self._recording = True self.simulationName = simulationName @@ -121,9 +121,10 @@ def beginRecording(self, config, simulationName, timestep, globalParams): assert val >= 0, val if unit == "steps": assert isinstance(val, int), val - self._delay = val + delay = val else: # unit == "seconds" - self._delay = max(0, math.floor(val / timestep)) + delay = max(0, math.floor(val / timestep)) + self._startTime = currentTime + delay def recordValue(self, value, step): raise NotImplementedError @@ -133,7 +134,8 @@ def endRecording(self, canceled): self._recording = False def _record(self, value, step): - if step >= self._delay and step % self._period == 0: + relativeTime = step - self._startTime + if relativeTime >= 0 and relativeTime % self._period == 0: self.recordValue(np.asarray(value), step) @staticmethod diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 3e9c0308f..9923e3d63 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -379,18 +379,11 @@ def __init__( # Run the simulation. terminationType, terminationReason = self._run(dynamicScenario, maxSteps) - # Stop all remaining scenarios. - # (and reject if some 'require eventually' condition was never satisfied) + # Stop all remaining scenarios (and handle their `record final` statements; + # also reject if some `require eventually` condition was never satisfied). for scenario in tuple(reversed(veneer.runningScenarios)): scenario._stop("simulation terminated") - # Record finally-recorded values. - values = dynamicScenario._evaluateRecordedExprs( - RequirementType.recordFinal, self.currentTime - ) - for name, val in values.items(): - self.records[name] = val - # Package up simulation results into a compact object. result = SimulationResult( self.trajectory, @@ -442,7 +435,7 @@ def _run(self, dynamicScenario, maxSteps): ) # Record current state of the simulation - self.recordCurrentState() + self._recordCurrentState() # Run monitors newReason = dynamicScenario._runMonitors() @@ -596,25 +589,18 @@ def createObjectInSimulator(self, obj): """ raise NotImplementedError - def recordCurrentState(self): - dynamicScenario = self.scene.dynamicScenario - records = self.records + def _recordCurrentState(self): + # Record values of `record initial` and `record` statements. + # (calls _record and _recordTimeSeries below) + self.scene.dynamicScenario._updateRecords() - # Record initially-recorded values - step = self.currentTime - if step == 0: - values = dynamicScenario._evaluateRecordedExprs( - RequirementType.recordInitial, step - ) - for name, val in values.items(): - records[name] = val + self.trajectory.append(self.currentState()) - # Record time-series values - values = dynamicScenario._evaluateRecordedExprs(RequirementType.record, step) - for name, val in values.items(): - records[name].append((self.currentTime, val)) + def _record(self, name, value): + self.records[name] = value - self.trajectory.append(self.currentState()) + def _recordTimeSeries(self, name, value): + self.records[name].append((self.currentTime, value)) def replayCanContinue(self): if not self.replaying: diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index b79745030..37994e21d 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -531,31 +531,27 @@ def executeInRequirement(scenario, boundEgo, values): assert activity == 0 assert not evaluatingRequirement evaluatingRequirement = True - if currentScenario is None: - currentScenario = scenario - clearScenario = True - else: - assert currentScenario is scenario - clearScenario = False - oldEgo = currentScenario._ego - oldObjects = currentScenario._objects - currentScenario._objects = tuple(values[obj] for obj in currentScenario.objects) + with executeInScenario(scenario): + oldEgo = scenario._ego + oldObjects = scenario._objects - if boundEgo: - currentScenario._ego = boundEgo - try: - yield - except RandomControlFlowError as e: - # Such errors should not be possible inside a requirement, since all values - # should have already been sampled: something's gone wrong with our rebinding. - raise RuntimeError("internal error: requirement dependency not sampled") from e - finally: - evaluatingRequirement = False - currentScenario._ego = oldEgo - currentScenario._objects = oldObjects - if clearScenario: - currentScenario = None + scenario._objects = tuple(values[obj] for obj in scenario.objects) + + if boundEgo: + scenario._ego = boundEgo + try: + yield + except RandomControlFlowError as e: + # Such errors should not be possible inside a requirement, since all values + # should have already been sampled: something's gone wrong with our rebinding. + raise RuntimeError( + "internal error: requirement dependency not sampled" + ) from e + finally: + evaluatingRequirement = False + scenario._ego = oldEgo + scenario._objects = oldObjects # Dynamic scenarios @@ -837,22 +833,6 @@ def record_final(reqID, value, line, name): makeRequirement(requirements.RequirementType.recordFinal, reqID, value, line, name) -def require_always(reqID, req, line, name): - """Function implementing the 'require always' statement.""" - if not name: - name = f"requirement on line {line}" - makeRequirement(requirements.RequirementType.requireAlways, reqID, req, line, name) - - -def require_eventually(reqID, req, line, name): - """Function implementing the 'require eventually' statement.""" - if not name: - name = f"requirement on line {line}" - makeRequirement( - requirements.RequirementType.requireEventually, reqID, req, line, name - ) - - def terminate_when(reqID, req, line, name): """Function implementing the 'terminate when' statement.""" if not name: @@ -874,9 +854,7 @@ def makeRequirement(ty, reqID, req, line, name, recConfig=None): raise InvalidScenarioError(f'tried to use "{ty.value}" inside a requirement') elif currentBehavior is not None: raise InvalidScenarioError(f'"{ty.value}" inside a behavior on line {line}') - elif currentSimulation is not None: - currentScenario._addDynamicRequirement(ty, req, line, name) - else: # requirement being defined at compile time + else: currentScenario._addRequirement(ty, reqID, req, line, name, 1, recConfig) diff --git a/tests/syntax/test_modular.py b/tests/syntax/test_modular.py index c77a54fce..21f1a3425 100644 --- a/tests/syntax/test_modular.py +++ b/tests/syntax/test_modular.py @@ -572,6 +572,24 @@ def test_subscenario_require_eventually(): assert result is None +def test_subscenario_require_eventually_2(): + """Variant of the above test using `terminate when` instead of `terminate after`.""" + scenario = compileScenic( + """ + scenario Main(): + compose: + do Sub() + wait + scenario Sub(): + ego = new Object + require eventually simulation().currentTime == 2 + terminate when simulation().currentTime == 1 + """ + ) + result = sampleResultOnce(scenario, maxSteps=2) + assert result is None + + def test_subscenario_require_monitor(): """Test that monitors invoked in subscenarios terminate with the subscenario.""" scenario = compileScenic( @@ -595,8 +613,42 @@ def test_subscenario_require_monitor(): assert len(result.trajectory) == 4 +def test_subscenario_record(): + scenario = compileScenic( + """ + scenario Main(): + setup: + record initial simulation().currentTime as mainInitial + record final simulation().currentTime as mainFinal + record simulation().currentTime as mainTime + compose: + wait for 2 steps + do Sub() + wait + scenario Sub(): + ego = new Object + record initial -simulation().currentTime as subInitial + record final -simulation().currentTime as subFinal + record -simulation().currentTime as subNegTime + terminate after 2 steps + """ + ) + result = sampleResult(scenario, maxSteps=5) + records = result.records + assert records["mainInitial"] == 0 + assert records["mainFinal"] == 5 + assert tuple(records["mainTime"]) == ((0, 0), (1, 1), (2, 2), (3, 3), (4, 4), (5, 5)) + assert records["subInitial"] == -2 + assert records["subFinal"] == -4 + assert tuple(records["subNegTime"]) == ((2, -2), (3, -3), (4, -4)) + + def test_subscenario_terminate_when(): - """Test that 'terminate when' and 'require' are properly handled.""" + """Test that 'terminate when' is properly handled. + + In particular, this catches a bug where `terminate when` in a subscenario was + interpreted as defining a requirement instead of a termination condition. + """ scenario = compileScenic( """ scenario Main(): @@ -605,12 +657,12 @@ def test_subscenario_terminate_when(): wait scenario Sub(): ego = new Object - require eventually simulation().currentTime == 2 terminate when simulation().currentTime == 1 """ ) - result = sampleResultOnce(scenario, maxSteps=2) - assert result is None + result = sampleResultOnce(scenario, maxSteps=3) + assert result is not None + assert len(result.trajectory) == 3 def test_subscenario_terminate_with_parent(): From b163047b795c57aab0184cb03e5cf3638ce7ff71 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 12:20:20 -0700 Subject: [PATCH 033/134] Added fix for lane order in backwards LaneGroups. --- src/scenic/domains/driving/roads.py | 2 +- src/scenic/formats/opendrive/xodr_parser.py | 2 +- tests/domains/driving/test_network.py | 13 +++++++++++++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index f06377c5a..3fecfaf86 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -987,7 +987,7 @@ def _currentFormatVersion(cls): :meta private: """ - return 35 + return 36 class DigestMismatchError(Exception): """Exception raised when loading a cached map not matching the original file.""" diff --git a/src/scenic/formats/opendrive/xodr_parser.py b/src/scenic/formats/opendrive/xodr_parser.py index 8175d2c83..cf274db09 100644 --- a/src/scenic/formats/opendrive/xodr_parser.py +++ b/src/scenic/formats/opendrive/xodr_parser.py @@ -1143,7 +1143,7 @@ def getEdges(forward): leftEdge=leftEdge, rightEdge=rightEdge, road=None, - lanes=tuple(backwardLanes), + lanes=tuple(reversed(backwardLanes)), curb=(backwardShoulder.rightEdge if backwardShoulder else rightEdge), sidewalk=backwardSidewalk, bikeLane=None, diff --git a/tests/domains/driving/test_network.py b/tests/domains/driving/test_network.py index b03061cd2..e3c6ee460 100644 --- a/tests/domains/driving/test_network.py +++ b/tests/domains/driving/test_network.py @@ -1,8 +1,11 @@ from pathlib import Path +import random import pytest +import shapely from scenic.core.distributions import RejectionException +from scenic.core.regions import toPolygon from scenic.domains.driving.roads import Intersection, Network from tests.domains.driving.conftest import mapFolder @@ -257,6 +260,16 @@ def test_sidewalk(network): assert network.elementAt(pt) is sw +def test_laneGroup_lane_order(network): + for _ in range(30): + lg = random.choice(network.laneGroups) + lane_0_dist = shapely.distance(toPolygon(lg.lanes[0]), toPolygon(lg.curb)) + lane_distances = [ + shapely.distance(toPolygon(lane), toPolygon(lg.curb)) for lane in lg.lanes + ] + assert all(lane_dist >= lane_0_dist - 0.1 for lane_dist in lane_distances) + + # --- Tests for cached network pickles --- From 4d6abc2bf0188512f7f860a02abfaf063fee7f77 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 13:31:07 -0700 Subject: [PATCH 034/134] Additional controller tweaks. --- src/scenic/domains/driving/behaviors.scenic | 22 +++++++++++++-------- src/scenic/domains/driving/controllers.py | 4 ++-- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 1ca88063a..0f835903e 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -42,22 +42,27 @@ behavior WalkForwardBehavior(): behavior ConstantThrottleBehavior(x): take SetThrottleAction(x) -def getFollowLanePath(obj, minPathDistance, preferStraight, path_metadata=None): +def getFollowLanePath(obj, minPathDistance, preferStraight, laneToFollow=None, path_metadata=None): import shapely import itertools - def mergeLineStrings(geoms): - return shapely.geometry.LineString(itertools.chain.from_iterable(geom.coords for geom in geoms)) + if laneToFollow is None: + laneToFollow = obj.lane + elif not isinstance(laneToFollow, Lane): + raise ValueError("`laneToFollow` is not a `Lane`.") if path_metadata is None: - current_lane = ego.lane - initial_path = obj.lane.centerline.lineString + current_lane = laneToFollow + initial_path = current_lane.centerline.lineString else: current_lane = path_metadata[0] initial_path = path_metadata[1] assert isinstance(initial_path, shapely.geometry.LineString) + def mergeLineStrings(geoms): + return shapely.geometry.LineString(itertools.chain.from_iterable(geom.coords for geom in geoms)) + ego_pt = shapely.geometry.Point(*obj.position) path = shapely.ops.substring(initial_path, initial_path.project(ego_pt), initial_path.length) @@ -84,7 +89,7 @@ def getFollowLanePath(obj, minPathDistance, preferStraight, path_metadata=None): assert isinstance(path, shapely.geometry.LineString) return PolylineRegion(polyline=path), (current_lane, path) -behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, preferStraight=True): +behavior FollowLaneBehavior(target_speed=10, laneToFollow=None, preferStraight=True): """ Follows the lane on which the vehicle is at, unless the laneToFollow is specified. Once the vehicle reaches an intersection, by default, the vehicle will take the straight route. @@ -108,7 +113,8 @@ behavior FollowLaneBehavior(target_speed = 10, laneToFollow=None, preferStraight while True: replan_time = 10 min_path_distance = max(2*replan_time*target_speed, 50) - path, path_metadata = getFollowLanePath(self, min_path_distance, preferStraight=preferStraight, path_metadata=path_metadata) + path, path_metadata = getFollowLanePath(self, min_path_distance, + preferStraight=preferStraight, laneToFollow=laneToFollow, path_metadata=path_metadata) traj = Trajectory.createFixedSpeedTrajectory(path, target_speed, ts=simulation().timestep) do FollowTrajectoryBehavior(traj) for replan_time seconds @@ -180,7 +186,7 @@ behavior FollowTrajectoryBehavior(trajectory, terminationDistance=1): throttle_action = SetBrakeAction(-throttle) # Compute steering : Lateral Control - steer = self.lateralController.computeSteering(trajectory, self, simulation()) + steer = self.lateralController.computeSteering(trajectory, self) steer_action = SetSteerAction(steer) take throttle_action, steer_action diff --git a/src/scenic/domains/driving/controllers.py b/src/scenic/domains/driving/controllers.py index 59ce92868..f45298186 100644 --- a/src/scenic/domains/driving/controllers.py +++ b/src/scenic/domains/driving/controllers.py @@ -102,7 +102,7 @@ def __init__(self, dt=0.1, *, K_P=0.3, K_D=0.2, K_I=0, wg=0): super().__init__(dt=dt, K_P=K_P, K_D=K_D, K_I=K_I, wg=wg) def computeSteering(self, trajectory, veh): - cte = trajectory.signedDistanceTo(veh.position) + cte = trajectory.polyline.signedDistanceTo(veh.position) steer_angle = self.run_step(cte) return steer_angle @@ -152,7 +152,7 @@ def _findTargetPoint(self, trajectory, veh, lookaheadDistance): self._lastTargetPoint = target_point return Vector(target_point.x, target_point.y) - def computeSteering(self, trajectory, veh, simulation): + def computeSteering(self, trajectory, veh): # Compute target steering angle lookaheadDistance = self.lookaheadDistance(veh) targetPoint = self._findTargetPoint(trajectory, veh, lookaheadDistance) From cdfef5bd0ee8b64099b20ea2fa6c96df7833e521 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 14:19:59 -0700 Subject: [PATCH 035/134] Added followFromTrajectory method. --- src/scenic/core/vectors.py | 30 +++++++++++++++++++++++++++++- tests/core/test_vectors.py | 18 ++++++++++++++++++ 2 files changed, 47 insertions(+), 1 deletion(-) diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index c4ae11f48..912a79e36 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -702,6 +702,24 @@ def __getitem__(self, pos) -> Orientation: val, f"value function of {self.name} returned non-orientation" ) + @distributionMethod + def followFromTrajectory(self, pos, dist, steps=None, stepSize=None): + """Follow the field from a point for a given distance, returning intermediate points. + + Uses the forward Euler approximation, covering the given distance with + equal-size steps. The number of steps can be given manually, or computed + automatically from a desired step size. + + Arguments: + pos (`Vector`): point to start from. + dist (float): distance to travel. + steps (int): number of steps to take, or :obj:`None` to compute the number of + steps based on the distance (default :obj:`None`). + stepSize (float): length used to compute how many steps to take, or + :obj:`None` to use the field's default step size. + """ + return self._followFromHelper(pos, dist, steps, stepSize, allPoints=True) + @vectorDistributionMethod def followFrom(self, pos, dist, steps=None, stepSize=None): """Follow the field from a point for a given distance. @@ -718,6 +736,14 @@ def followFrom(self, pos, dist, steps=None, stepSize=None): stepSize (float): length used to compute how many steps to take, or :obj:`None` to use the field's default step size. """ + return self._followFromHelper(pos, dist, steps, stepSize, allPoints=False) + + def _followFromHelper(self, pos, dist, steps, stepSize, allPoints): + if allPoints: + from scenic.core.regions import PolylineRegion + + pts = [] + if steps is None: steps = self.minSteps stepSize = self.defaultStepSize if stepSize is None else stepSize @@ -729,8 +755,10 @@ def followFrom(self, pos, dist, steps=None, stepSize=None): for i in range(steps): rot = self[pos].getRotation() pos += rot.apply(step) + if allPoints: + pts.append(Vector(*pos)) - return Vector(*pos) + return PolylineRegion(points=pts) if allPoints else Vector(*pos) @staticmethod def forUnionOf(regions, tolerance=0): diff --git a/tests/core/test_vectors.py b/tests/core/test_vectors.py index 82f8be7ff..a48f959be 100644 --- a/tests/core/test_vectors.py +++ b/tests/core/test_vectors.py @@ -69,3 +69,21 @@ def test_distribution_method_encapsulation_lazy(): assert not needsLazyEvaluation(evpt) assert isinstance(evpt, VectorMethodDistribution) assert evpt.method is underlyingFunction(vf.followFrom) + + +def test_vf_follow(): + vf = VectorField( + "Foo", lambda pos: 0 if int(pos.x + pos.y) % 2 == 0 else (math.radians(-90)) + ) + + start_pt = Vector(0, 0) + end_pt = vf.followFrom(start_pt, 5, stepSize=1) + traj = vf.followFromTrajectory(start_pt, 5, stepSize=1) + + assert traj.points[-1] == pytest.approx(end_pt) + + for pt_a, pt_b in itertools.pairwise(traj.points): + if int(sum(pt_a)) % 2 == 0: + assert pt_a + Vector(0, 1) == pytest.approx(pt_b) + else: + assert pt_a + Vector(1, 0) == pytest.approx(pt_b) From 9b5243edada1d2efdfbf5be0e780d19614ddedd7 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 14:23:27 -0700 Subject: [PATCH 036/134] Fix docs building. --- src/scenic/domains/driving/behaviors.scenic | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 0f835903e..85e00546a 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -163,14 +163,14 @@ class Trajectory(object): behavior FollowTrajectoryBehavior(trajectory, terminationDistance=1): """ - Follows the given `Trajectory`. + Follows the given Trajectory. The behavior terminates when either of the following conditions are met the vehicle position is within - `terminationDistance` of the end of the trajectory. + terminationDistance of the end of the trajectory. Args: - trajectory: A `Trajectory`. - terminationDistance: The behavior will terminate when the vehicle position is within `terminationDistance` + trajectory: A Trajectory. + terminationDistance: The behavior will terminate when the vehicle position is within terminationDistance of the end of the trajectory. """ assert isinstance(trajectory, Trajectory) From 5924262c78aa7979e602eebe166ac292173f7749 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 14:40:13 -0700 Subject: [PATCH 037/134] Added setOrientation functionality to driving domain --- src/scenic/domains/driving/actions.py | 11 +++++++++++ src/scenic/domains/driving/model.scenic | 3 +++ 2 files changed, 14 insertions(+) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index f2ab406d1..d8304ea9c 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -18,6 +18,7 @@ import numpy as np from scenic.core.simulators import Action +from scenic.core.type_support import toOrientation from scenic.core.vectors import Vector ## Mixin classes indicating support for various types of actions. @@ -108,6 +109,16 @@ def applyTo(self, obj, sim): obj.setVelocity(vel) +class SetOrientationAction(Action): + """Set the orientation of an agent.""" + + def __init__(self, orientation): + self.orientation = toOrientation(orientation) + + def applyTo(self, obj, sim): + obj.setOrientation(self.orientation) + + ## Actions available to vehicles which can steer diff --git a/src/scenic/domains/driving/model.scenic b/src/scenic/domains/driving/model.scenic index b13298c51..3526a8990 100644 --- a/src/scenic/domains/driving/model.scenic +++ b/src/scenic/domains/driving/model.scenic @@ -270,6 +270,9 @@ class DrivingObject: def setVelocity(self, vel): raise NotImplementedError + def setOrientation(self, orientation): + raise NotImplementedError + class Vehicle(DrivingObject): """Vehicles which drive, such as cars. From ebc20b1580c490bd84f72e653ee7821192bd1ad5 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 15:33:09 -0700 Subject: [PATCH 038/134] Added python floor to pairwise test. --- tests/core/test_vectors.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tests/core/test_vectors.py b/tests/core/test_vectors.py index a48f959be..b9768b1bd 100644 --- a/tests/core/test_vectors.py +++ b/tests/core/test_vectors.py @@ -71,6 +71,9 @@ def test_distribution_method_encapsulation_lazy(): assert evpt.method is underlyingFunction(vf.followFrom) +@pytest.mark.skipif( + sys.version_info < (3, 10), reason="Pairwise requires Python 3.10 or higher." +) def test_vf_follow(): vf = VectorField( "Foo", lambda pos: 0 if int(pos.x + pos.y) % 2 == 0 else (math.radians(-90)) From 2887b04b28636c45de43bbae37e47ddb12f7f15d Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 16:53:32 -0700 Subject: [PATCH 039/134] Prepare for `apparent heading to` syntax. --- src/scenic/syntax/ast.py | 2 +- src/scenic/syntax/compiler.py | 4 ++-- src/scenic/syntax/scenic.gram | 4 ++-- src/scenic/syntax/veneer.py | 4 ++-- tests/syntax/test_compiler.py | 8 ++++---- tests/syntax/test_parser.py | 30 ++++++++++++++++-------------- 6 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/scenic/syntax/ast.py b/src/scenic/syntax/ast.py index f21a49aaf..ed11764fd 100644 --- a/src/scenic/syntax/ast.py +++ b/src/scenic/syntax/ast.py @@ -435,7 +435,7 @@ class RelativeHeadingOp(AST): base: Optional[ast.AST] = None -class ApparentHeadingOp(AST): +class ApparentHeadingOfOp(AST): target: ast.AST base: Optional[ast.AST] = None diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index ac1beeae9..f9f50c5b2 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1683,9 +1683,9 @@ def visit_RelativeHeadingOp(self, node: s.RelativeHeadingOp): ), ) - def visit_ApparentHeadingOp(self, node: s.ApparentHeadingOp): + def visit_ApparentHeadingOfOp(self, node: s.ApparentHeadingOfOp): return ast.Call( - func=ast.Name(id="ApparentHeading", ctx=loadCtx), + func=ast.Name(id="ApparentHeadingOf", ctx=loadCtx), args=[self.visit(node.target)], keywords=( [] diff --git a/src/scenic/syntax/scenic.gram b/src/scenic/syntax/scenic.gram index ec3e63f10..316e8cabb 100644 --- a/src/scenic/syntax/scenic.gram +++ b/src/scenic/syntax/scenic.gram @@ -1846,8 +1846,8 @@ scenic_prefix_operators: | "relative" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.RelativeHeadingOp(target=e1, base=e2, LOCATIONS) } | "relative" "heading" "of" e1=scenic_prefix_operators { s.RelativeHeadingOp(target=e1, LOCATIONS) } # apparent heading of - | "apparent" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingOp(target=e1, base=e2, LOCATIONS) } - | "apparent" "heading" "of" e1=scenic_prefix_operators { s.ApparentHeadingOp(target=e1, LOCATIONS) } + | "apparent" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingOfOp(target=e1, base=e2, LOCATIONS) } + | "apparent" "heading" "of" e1=scenic_prefix_operators { s.ApparentHeadingOfOp(target=e1, LOCATIONS) } # distance from/to | &"distance" scenic_distance_from_op # distance past diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index b79745030..28605ba4e 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -65,7 +65,7 @@ "BottomBackLeft", "BottomBackRight", "RelativeHeading", - "ApparentHeading", + "ApparentHeadingOf", "RelativePosition", "DistanceFrom", "DistancePast", @@ -1275,7 +1275,7 @@ def RelativeHeading(X, Y=None): return normalizeAngle(X.yaw - Y.yaw) -def ApparentHeading(X, Y=None): +def ApparentHeadingOf(X, Y=None): """The :grammar:`apparent heading of [from ]` operator. If the :grammar:`from ` is omitted, the position of ego is used. diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 597b85b55..8bc3b6375 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -2076,17 +2076,17 @@ def test_relative_heading_op_base(self): assert False def test_apparent_heading_op(self): - node, _ = compileScenicAST(ApparentHeadingOp(Name("X"))) + node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"))) match node: - case Call(Name("ApparentHeading"), [Name("X")]): + case Call(Name("ApparentHeadingOf"), [Name("X")]): assert True case _: assert False def test_apparent_heading_op_base(self): - node, _ = compileScenicAST(ApparentHeadingOp(Name("X"), Name("Y"))) + node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"), Name("Y"))) match node: - case Call(Name("ApparentHeading"), [Name("X")], [keyword("Y", Name("Y"))]): + case Call(Name("ApparentHeadingOf"), [Name("X")], [keyword("Y", Name("Y"))]): assert True case _: assert False diff --git a/tests/syntax/test_parser.py b/tests/syntax/test_parser.py index b259b2ad0..d50379722 100644 --- a/tests/syntax/test_parser.py +++ b/tests/syntax/test_parser.py @@ -2222,7 +2222,7 @@ def test_apparent_heading(self): mod = parse_string_helper("apparent heading of x") stmt = mod.body[0] match stmt: - case Expr(ApparentHeadingOp(Name("x"))): + case Expr(ApparentHeadingOfOp(Name("x"))): assert True case _: assert False @@ -2231,7 +2231,7 @@ def test_apparent_heading_from(self): mod = parse_string_helper("apparent heading of x from y") stmt = mod.body[0] match stmt: - case Expr(ApparentHeadingOp(Name("x"), Name("y"))): + case Expr(ApparentHeadingOfOp(Name("x"), Name("y"))): assert True case _: assert False @@ -2241,27 +2241,27 @@ def test_apparent_heading_from(self): [ ( "apparent heading of apparent heading of A from B", - ApparentHeadingOp( - ApparentHeadingOp(Name("A", Load()), Name("B", Load())) + ApparentHeadingOfOp( + ApparentHeadingOfOp(Name("A", Load()), Name("B", Load())) ), ), ( "apparent heading of apparent heading of A from B from C", - ApparentHeadingOp( - ApparentHeadingOp(Name("A", Load()), Name("B", Load())), + ApparentHeadingOfOp( + ApparentHeadingOfOp(Name("A", Load()), Name("B", Load())), Name("C", Load()), ), ), ( "apparent heading of A from apparent heading of B from C", - ApparentHeadingOp( + ApparentHeadingOfOp( Name("A", Load()), - ApparentHeadingOp(Name("B", Load()), Name("C", Load())), + ApparentHeadingOfOp(Name("B", Load()), Name("C", Load())), ), ), ( "apparent heading of A << B from C", - ApparentHeadingOp( + ApparentHeadingOfOp( BinOp(Name("A", Load()), LShift(), Name("B", Load())), Name("C", Load()), ), @@ -2269,32 +2269,34 @@ def test_apparent_heading_from(self): ( "apparent heading of A from B << C", BinOp( - ApparentHeadingOp(Name("A", Load()), Name("B", Load())), + ApparentHeadingOfOp(Name("A", Load()), Name("B", Load())), LShift(), Name("C", Load()), ), ), ( "apparent heading of A + B from C", - ApparentHeadingOp( + ApparentHeadingOfOp( BinOp(Name("A", Load()), Add(), Name("B", Load())), Name("C", Load()), ), ), ( "apparent heading of A from B + C", - ApparentHeadingOp( + ApparentHeadingOfOp( Name("A", Load()), BinOp(Name("B", Load()), Add(), Name("C", Load())), ), ), ( "apparent heading of A << B", - BinOp(ApparentHeadingOp(Name("A", Load())), LShift(), Name("B", Load())), + BinOp( + ApparentHeadingOfOp(Name("A", Load())), LShift(), Name("B", Load()) + ), ), ( "apparent heading of A + B", - ApparentHeadingOp(BinOp(Name("A", Load()), Add(), Name("B", Load()))), + ApparentHeadingOfOp(BinOp(Name("A", Load()), Add(), Name("B", Load()))), ), ], ) From 6c89a4a540a11746f77da66ab21ac97416d5e4de Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 6 Jul 2026 17:20:09 -0700 Subject: [PATCH 040/134] Added `apparent heading to X from Y` operator. --- src/scenic/core/object_types.py | 4 + src/scenic/domains/driving/behaviors.scenic | 1 + src/scenic/syntax/ast.py | 5 ++ src/scenic/syntax/compiler.py | 11 +++ src/scenic/syntax/scenic.gram | 3 + src/scenic/syntax/veneer.py | 16 +++- tests/syntax/test_compiler.py | 20 ++++- tests/syntax/test_operators.py | 40 ++++++++- tests/syntax/test_parser.py | 94 ++++++++++++++++++++- 9 files changed, 185 insertions(+), 9 deletions(-) diff --git a/src/scenic/core/object_types.py b/src/scenic/core/object_types.py index 11ff01004..1d648252d 100644 --- a/src/scenic/core/object_types.py +++ b/src/scenic/core/object_types.py @@ -988,6 +988,10 @@ def distancePast(self, vec): diff = self.position - vec return diff.rotatedBy(-self.heading).y + def apparentHeadingTo(self, vec): + """The apparent heading to a given point, from the perspective of this `OrientedPoint`.""" + return normalizeAngle(self.position.angleTo(vec) - self.heading) + def toHeading(self) -> float: return self.heading diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 85e00546a..5db4794a6 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -5,6 +5,7 @@ These behaviors are automatically imported when using the driving domain. import math from abc import ABC, abstractmethod +import warnings import shapely from shapely.geometry import LineString, MultiPoint, Point as ShapelyPoint diff --git a/src/scenic/syntax/ast.py b/src/scenic/syntax/ast.py index ed11764fd..35664adb1 100644 --- a/src/scenic/syntax/ast.py +++ b/src/scenic/syntax/ast.py @@ -440,6 +440,11 @@ class ApparentHeadingOfOp(AST): base: Optional[ast.AST] = None +class ApparentHeadingToOp(AST): + target: ast.AST + base: Optional[ast.AST] = None + + class DistanceFromOp(AST): # because `to` and `from` are symmetric, the first operand will be `target` and the second will be `base` target: ast.AST diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index f9f50c5b2..3daa536cd 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1694,6 +1694,17 @@ def visit_ApparentHeadingOfOp(self, node: s.ApparentHeadingOfOp): ), ) + def visit_ApparentHeadingToOp(self, node: s.ApparentHeadingToOp): + return ast.Call( + func=ast.Name(id="ApparentHeadingTo", ctx=loadCtx), + args=[self.visit(node.target)], + keywords=( + [] + if node.base is None + else [ast.keyword(arg="Y", value=self.visit(node.base))] + ), + ) + def visit_DistanceFromOp(self, node: s.DistanceFromOp): return ast.Call( func=ast.Name(id="DistanceFrom", ctx=loadCtx), diff --git a/src/scenic/syntax/scenic.gram b/src/scenic/syntax/scenic.gram index 316e8cabb..d573c031b 100644 --- a/src/scenic/syntax/scenic.gram +++ b/src/scenic/syntax/scenic.gram @@ -1848,6 +1848,9 @@ scenic_prefix_operators: # apparent heading of | "apparent" "heading" "of" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingOfOp(target=e1, base=e2, LOCATIONS) } | "apparent" "heading" "of" e1=scenic_prefix_operators { s.ApparentHeadingOfOp(target=e1, LOCATIONS) } + # apparent heading to + | "apparent" "heading" "to" e1=expression 'from' e2=scenic_prefix_operators { s.ApparentHeadingToOp(target=e1, base=e2, LOCATIONS) } + | "apparent" "heading" "to" e1=scenic_prefix_operators { s.ApparentHeadingToOp(target=e1, LOCATIONS) } # distance from/to | &"distance" scenic_distance_from_op # distance past diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index 28605ba4e..bc987d0f4 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -66,6 +66,7 @@ "BottomBackRight", "RelativeHeading", "ApparentHeadingOf", + "ApparentHeadingTo", "RelativePosition", "DistanceFrom", "DistancePast", @@ -1284,10 +1285,23 @@ def ApparentHeadingOf(X, Y=None): raise TypeError('"apparent heading of X from Y" with X not an OrientedPoint') if Y is None: Y = ego() - Y = toVector(Y, '"relative heading of X from Y" with Y not a vector') + Y = toVector(Y, '"apparent heading of X from Y" with Y not a vector') return apparentHeadingAtPoint(X.position, X.heading, Y) +def ApparentHeadingTo(X, Y=None): + """The :grammar:`apparent heading to [from ]` operator. + + If the :grammar:`from ` is omitted, the ego is used. + """ + X = toVector(X, '"apparent heading to X from Y" with X not a vector') + if Y is None: + Y = ego() + if not isA(Y, OrientedPoint): + raise TypeError('"apparent heading to X from Y" with Y not an OrientedPoint') + return Y.apparentHeadingTo(X) + + def DistanceFrom(X, Y=None): """The :scenic:`distance from {X} to {Y}` polymorphic operator. diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 8bc3b6375..2100c364a 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -2075,7 +2075,7 @@ def test_relative_heading_op_base(self): case _: assert False - def test_apparent_heading_op(self): + def test_apparent_heading_of_op(self): node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"))) match node: case Call(Name("ApparentHeadingOf"), [Name("X")]): @@ -2083,7 +2083,7 @@ def test_apparent_heading_op(self): case _: assert False - def test_apparent_heading_op_base(self): + def test_apparent_heading_of_op_base(self): node, _ = compileScenicAST(ApparentHeadingOfOp(Name("X"), Name("Y"))) match node: case Call(Name("ApparentHeadingOf"), [Name("X")], [keyword("Y", Name("Y"))]): @@ -2091,6 +2091,22 @@ def test_apparent_heading_op_base(self): case _: assert False + def test_apparent_heading_to_op(self): + node, _ = compileScenicAST(ApparentHeadingToOp(Name("X"))) + match node: + case Call(Name("ApparentHeadingTo"), [Name("X")]): + assert True + case _: + assert False + + def test_apparent_heading_to_op_base(self): + node, _ = compileScenicAST(ApparentHeadingToOp(Name("X"), Name("Y"))) + match node: + case Call(Name("ApparentHeadingTo"), [Name("X")], [keyword("Y", Name("Y"))]): + assert True + case _: + assert False + def test_distance_to_op(self): node, _ = compileScenicAST(DistanceFromOp(Name("X"), None)) match node: diff --git a/tests/syntax/test_operators.py b/tests/syntax/test_operators.py index 7c405c789..dd3e9d16f 100644 --- a/tests/syntax/test_operators.py +++ b/tests/syntax/test_operators.py @@ -43,8 +43,8 @@ def test_relative_heading_from(): assert ego.heading == pytest.approx(math.radians(70 + 10)) -# Apparent heading -def test_apparent_heading(): +# Apparent heading of +def test_apparent_heading_of(): p = sampleParamPFrom( """ ego = new Object facing 30 deg @@ -55,7 +55,7 @@ def test_apparent_heading(): assert p == pytest.approx(math.radians(65 + 45)) -def test_apparent_heading_no_ego(): +def test_apparent_heading_of_no_ego(): with pytest.raises(InvalidScenarioError): compileScenic( """ @@ -65,7 +65,7 @@ def test_apparent_heading_no_ego(): ) -def test_apparent_heading_from(): +def test_apparent_heading_of_from(): ego = sampleEgoFrom( """ OP = new OrientedPoint at 10@15, facing -60 deg @@ -75,6 +75,38 @@ def test_apparent_heading_from(): assert ego.heading == pytest.approx(math.radians(-60 - 45)) +def test_apparent_heading_to(): + p = sampleParamPFrom( + """ + ego = new Object facing 30 deg + other = new Object facing 65 deg, at 10@10 + param p = apparent heading to other + """ + ) + assert p == pytest.approx(math.radians(-30 - 45)) + + +def test_apparent_heading_to_no_ego(): + with pytest.raises(InvalidScenarioError): + compileScenic( + """ + other = new Object + ego = new Object at 2@2, facing apparent heading to other + """ + ) + + +def test_apparent_heading_to_from(): + p = sampleParamPFrom( + """ + foo = new Object facing 30 deg + other = new Object facing 65 deg, at 10@10 + param p = apparent heading to other from foo + """ + ) + assert p == pytest.approx(math.radians(-30 - 45)) + + # Angle def test_angle(): p = sampleParamPFrom( diff --git a/tests/syntax/test_parser.py b/tests/syntax/test_parser.py index d50379722..c0430904a 100644 --- a/tests/syntax/test_parser.py +++ b/tests/syntax/test_parser.py @@ -2218,7 +2218,7 @@ def test_relative_heading_from(self): def test_relative_heading_precedence(self, code, expected): assert_equal_source_ast(code, expected) - def test_apparent_heading(self): + def test_apparent_heading_of(self): mod = parse_string_helper("apparent heading of x") stmt = mod.body[0] match stmt: @@ -2227,7 +2227,7 @@ def test_apparent_heading(self): case _: assert False - def test_apparent_heading_from(self): + def test_apparent_heading_of_from(self): mod = parse_string_helper("apparent heading of x from y") stmt = mod.body[0] match stmt: @@ -2303,6 +2303,96 @@ def test_apparent_heading_from(self): def test_apparent_heading_precedence(self, code, expected): assert_equal_source_ast(code, expected) + def test_apparent_heading_to_from(self): + mod = parse_string_helper("apparent heading to x from y") + stmt = mod.body[0] + match stmt: + case Expr(ApparentHeadingToOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + @pytest.mark.parametrize( + "code,expected", + [ + ( + "apparent heading to apparent heading to A from B", + ApparentHeadingToOp( + ApparentHeadingToOp(Name("A", Load()), Name("B", Load())) + ), + ), + ( + "apparent heading to apparent heading to A from B from C", + ApparentHeadingToOp( + ApparentHeadingToOp(Name("A", Load()), Name("B", Load())), + Name("C", Load()), + ), + ), + ( + "apparent heading to A from apparent heading to B from C", + ApparentHeadingToOp( + Name("A", Load()), + ApparentHeadingToOp(Name("B", Load()), Name("C", Load())), + ), + ), + ( + "apparent heading of A from apparent heading to B from C", + ApparentHeadingOfOp( + Name("A", Load()), + ApparentHeadingToOp(Name("B", Load()), Name("C", Load())), + ), + ), + ( + "apparent heading to A from apparent heading of B from C", + ApparentHeadingToOp( + Name("A", Load()), + ApparentHeadingOfOp(Name("B", Load()), Name("C", Load())), + ), + ), + ( + "apparent heading to A << B from C", + ApparentHeadingToOp( + BinOp(Name("A", Load()), LShift(), Name("B", Load())), + Name("C", Load()), + ), + ), + ( + "apparent heading to A from B << C", + BinOp( + ApparentHeadingToOp(Name("A", Load()), Name("B", Load())), + LShift(), + Name("C", Load()), + ), + ), + ( + "apparent heading to A + B from C", + ApparentHeadingToOp( + BinOp(Name("A", Load()), Add(), Name("B", Load())), + Name("C", Load()), + ), + ), + ( + "apparent heading to A from B + C", + ApparentHeadingToOp( + Name("A", Load()), + BinOp(Name("B", Load()), Add(), Name("C", Load())), + ), + ), + ( + "apparent heading to A << B", + BinOp( + ApparentHeadingToOp(Name("A", Load())), LShift(), Name("B", Load()) + ), + ), + ( + "apparent heading to A + B", + ApparentHeadingToOp(BinOp(Name("A", Load()), Add(), Name("B", Load()))), + ), + ], + ) + def test_apparent_heading_precedence(self, code, expected): + assert_equal_source_ast(code, expected) + def test_distance_from(self): mod = parse_string_helper("distance from x") stmt = mod.body[0] From d2cd9e33e8fca1d3b986292298bc81e1edd2aa30 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 7 Jul 2026 10:06:45 -0700 Subject: [PATCH 041/134] Added documentation. --- docs/reference/operators.rst | 15 +++++++++ src/scenic/syntax/ast.py | 20 ++++++++++++ src/scenic/syntax/compiler.py | 40 ++++++++++++++++++++++++ src/scenic/syntax/scenic.gram | 4 +++ src/scenic/syntax/veneer.py | 43 ++++++++++++++++++++++++++ tests/syntax/test_compiler.py | 32 +++++++++++++++++++ tests/syntax/test_operators.py | 56 ++++++++++++++++++++++++++++++++++ tests/syntax/test_parser.py | 36 ++++++++++++++++++++++ 8 files changed, 246 insertions(+) diff --git a/docs/reference/operators.rst b/docs/reference/operators.rst index 4a39539e9..0273979b6 100644 --- a/docs/reference/operators.rst +++ b/docs/reference/operators.rst @@ -28,6 +28,12 @@ apparent heading of *OrientedPoint* [from *vector*] --------------------------------------------------- The apparent heading of the OrientedPoint, with respect to the line of sight from ego (or the position provided with the optional from vector) +.. _apparent heading to {vector} [from {OrientedPoint}]: + +apparent heading to *vector* [from *OrientedPoint*] +--------------------------------------------------- +The apparent heading to the vector, with respect to the line of sight from ego (or the OrientedPoint provided with the optional from OrientedPoint) + .. _distance [from {vector}] to {vector}: .. _distance from: @@ -75,6 +81,15 @@ Whether an `Object`/`Region` intersects another `Object`/`Region`, i.e. whether When working with 2D regions, it can be useful to check intersection with the :term:`footprint` of a region, e.g. when checking whether a car intersects a given lane. In this case, one would write :scenic:`car intersects lane.footprint` instead of :scenic:`car intersects lane`. For more details, see :term:`footprint`. +.. _{vector} (ahead of | behind | left of | right of) {region}: + +*vector* (ahead of | behind | left of | right of) *OrientedPoint* +----------------------------------------------------------------- +Whether a vector is to a respective side of an `OrientedPoint` (i.e. within +- ~90 degrees from that direction), accounting for the heading of the `OrientedPoint`. These operators are convenient shorthand for comparing :sampref:`apparent heading to {vector} [from {OrientedPoint}]` to a range of values. + +Note: For vectors very close to a boundary point (i.e. an apparent heading to the vector in (89.99, 90.01) or (-89.99, -90.01) for "ahead of" and "behind"), both operators will resolve to False. + +Example: If an `OrientedPoint` had an apparent heading to a vector of 45 degrees, it would be both "ahead of" and "left of" the `OrientedPoint` but not "behind" or "right of". Orientation Operators ===================== diff --git a/src/scenic/syntax/ast.py b/src/scenic/syntax/ast.py index 35664adb1..56eca6a1f 100644 --- a/src/scenic/syntax/ast.py +++ b/src/scenic/syntax/ast.py @@ -655,3 +655,23 @@ class CanSeeOp(AST): class IntersectsOp(AST): left: ast.AST right: ast.AST + + +class AheadOfOp(AST): + left: ast.AST + right: ast.AST + + +class BehindOp(AST): + left: ast.AST + right: ast.AST + + +class LeftOfOp(AST): + left: ast.AST + right: ast.AST + + +class RightOfOp(AST): + left: ast.AST + right: ast.AST diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index 3daa536cd..4e03ce67b 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1861,3 +1861,43 @@ def visit_IntersectsOp(self, node: s.IntersectsOp): ], keywords=[], ) + + def visit_AheadOfOp(self, node: s.AheadOfOp): + return ast.Call( + func=ast.Name(id="AheadOfOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) + + def visit_BehindOp(self, node: s.BehindOp): + return ast.Call( + func=ast.Name(id="BehindOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) + + def visit_LeftOfOp(self, node: s.LeftOfOp): + return ast.Call( + func=ast.Name(id="LeftOfOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) + + def visit_RightOfOp(self, node: s.RightOfOp): + return ast.Call( + func=ast.Name(id="RightOfOp", ctx=loadCtx), + args=[ + self.visit(node.left), + self.visit(node.right), + ], + keywords=[], + ) diff --git a/src/scenic/syntax/scenic.gram b/src/scenic/syntax/scenic.gram index d573c031b..31377a6de 100644 --- a/src/scenic/syntax/scenic.gram +++ b/src/scenic/syntax/scenic.gram @@ -1803,6 +1803,10 @@ bitwise_or: | scenic_not_visible_from | scenic_can_see | scenic_intersects + | a=bitwise_or "ahead" "of" b=bitwise_xor { s.AheadOfOp(left=a, right=b, LOCATIONS) } + | a=bitwise_or "behind" b=bitwise_xor { s.BehindOp(left=a, right=b, LOCATIONS) } + | a=bitwise_or "left" "of" b=bitwise_xor { s.LeftOfOp(left=a, right=b, LOCATIONS) } + | a=bitwise_or "right" "of" b=bitwise_xor { s.RightOfOp(left=a, right=b, LOCATIONS) } | a=bitwise_or '|' b=bitwise_xor { ast.BinOp(left=a, op=ast.BitOr(), right=b, LOCATIONS) } | bitwise_xor diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index bc987d0f4..d171dbeb3 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -85,6 +85,10 @@ "Implies", "VisibleFromOp", "NotVisibleFromOp", + "AheadOfOp", + "BehindOp", + "LeftOfOp", + "RightOfOp", # Primitive types "Vector", "Orientation", @@ -250,6 +254,7 @@ from contextlib import contextmanager import functools import importlib +import math import numbers from pathlib import Path import sys @@ -1401,6 +1406,44 @@ def NotVisibleFromOp(region, base): return region.difference(base.visibleRegion) +def AheadOfOp(X, Y): + """The :grammar:` ahead of ` operator.""" + X = toVector(X, '"X ahead of Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X ahead of Y" with Y not an OrientedPoint') + + return math.radians(-89.99) < ApparentHeadingTo(X, Y) < math.radians(89.99) + + +def BehindOp(X, Y): + """The :grammar:` behind ` operator.""" + X = toVector(X, '"X behind Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X behind Y" with Y not an OrientedPoint') + + return ApparentHeadingTo(X, Y) < -math.radians(90.01) or math.radians( + 90.01 + ) < ApparentHeadingTo(X, Y) + + +def LeftOfOp(X, Y): + """The :grammar:` left of ` operator.""" + X = toVector(X, '"X left of Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X left of Y" with Y not an OrientedPoint') + + return math.radians(0.01) < ApparentHeadingTo(X, Y) < math.radians(180) + + +def RightOfOp(X, Y): + """The :grammar:` right of ` operator.""" + X = toVector(X, '"X right of Y" with X not a vector') + if not isA(Y, OrientedPoint): + raise TypeError('"X right of Y" with Y not an OrientedPoint') + + return -math.radians(180) < ApparentHeadingTo(X, Y) < math.radians(-0.01) + + def CanSee(X, Y): """The :scenic:`{X} can see {Y}` polymorphic operator. diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 2100c364a..d4abc5956 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -2306,6 +2306,38 @@ def test_offset_along_op(self): case _: assert False + def test_ahead_of_op(self): + node, _ = compileScenicAST(AheadOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("AheadOfOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + + def test_behind_op(self): + node, _ = compileScenicAST(BehindOp(Name("X"), Name("Y"))) + match node: + case Call(Name("BehindOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + + def test_left_of_op(self): + node, _ = compileScenicAST(LeftOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("LeftOfOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + + def test_right_of_op(self): + node, _ = compileScenicAST(RightOfOp(Name("X"), Name("Y"))) + match node: + case Call(Name("RightOfOp"), [Name("X"), Name("Y")]): + assert True + case _: + assert False + def test_can_see_op(self): node, _ = compileScenicAST(CanSeeOp(Name("X"), Name("Y"))) match node: diff --git a/tests/syntax/test_operators.py b/tests/syntax/test_operators.py index dd3e9d16f..1817b0dc6 100644 --- a/tests/syntax/test_operators.py +++ b/tests/syntax/test_operators.py @@ -634,6 +634,62 @@ def test_intersects_diff_z(): assert p == (True, False, False) +def test_ahead_of_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo ahead of ego + """ + ) + for i in range(8) + ] + assert p_vals == [True, True, False, False, False, False, False, True] + + +def test_behind_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo behind ego + """ + ) + for i in range(8) + ] + assert p_vals == [False, False, False, True, True, True, False, False] + + +def test_left_of_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo left of ego + """ + ) + for i in range(8) + ] + assert p_vals == [False, True, True, True, False, False, False, False] + + +def test_right_of_op(): + p_vals = [ + sampleParamPFrom( + f""" + ego = new Object + foo = new Point offset along {i*45} deg by 0@1 + param p = foo right of ego + """ + ) + for i in range(8) + ] + assert p_vals == [False, False, False, False, False, True, True, True] + + ## Heading operators diff --git a/tests/syntax/test_parser.py b/tests/syntax/test_parser.py index c0430904a..1394778c0 100644 --- a/tests/syntax/test_parser.py +++ b/tests/syntax/test_parser.py @@ -3097,6 +3097,42 @@ def test_can_see(self): case _: assert False + def test_ahead_of(self): + mod = parse_string_helper("x ahead of y ") + stmt = mod.body[0] + match stmt: + case Expr(AheadOfOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_behind(self): + mod = parse_string_helper("x behind y ") + stmt = mod.body[0] + match stmt: + case Expr(BehindOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_left_of(self): + mod = parse_string_helper("x left of y ") + stmt = mod.body[0] + match stmt: + case Expr(LeftOfOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_right_of(self): + mod = parse_string_helper("x right of y ") + stmt = mod.body[0] + match stmt: + case Expr(RightOfOp(Name("x"), Name("y"))): + assert True + case _: + assert False + def test_intersects(self): mod = parse_string_helper("x intersects y ") stmt = mod.body[0] From 7bc9cbc96343b31129167325ad0d157ff6ed97c7 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 7 Jul 2026 12:38:52 -0700 Subject: [PATCH 042/134] Added logic for dynamically rebuilding parser on changes. --- .gitignore | 3 ++- src/scenic/syntax/__init__.py | 24 +++++++++++++++++++++++- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index aa502d3e5..c1288f49a 100644 --- a/.gitignore +++ b/.gitignore @@ -137,8 +137,9 @@ dmypy.json *.cproject -# generated parser +# generated parser and checksum src/scenic/syntax/parser.py +src/scenic/syntax/.parser_checksum # generated media/output simulation.gif diff --git a/src/scenic/syntax/__init__.py b/src/scenic/syntax/__init__.py index 9ea3b509d..8c9041b80 100644 --- a/src/scenic/syntax/__init__.py +++ b/src/scenic/syntax/__init__.py @@ -1,5 +1,6 @@ """The Scenic compiler and associated support code.""" +import hashlib as _hashlib import pathlib as _pathlib import subprocess as _subprocess import sys as _sys @@ -8,6 +9,7 @@ _projectRootDir = _syntaxDir.parent.parent.parent _grammarPath = _syntaxDir / "scenic.gram" _parserPath = _syntaxDir / "parser.py" +_checksumPath = _syntaxDir / ".parser_checksum" def buildParser(): @@ -30,10 +32,30 @@ def buildParser(): capture_output=True, text=True, ) + + with open(_checksumPath, "wb") as f: + f.write(getParserHash()) + return result -if not _parserPath.exists(): +def getParserHash(): + with open(_parserPath, "rb") as f: + data = f.read() + return _hashlib.blake2b(data).digest() + + +def checksumValid(): + if not _checksumPath.exists(): + return False + + with open(_checksumPath, "rb") as f: + checksum = f.read() + + return checksum == getParserHash() + + +if not _parserPath.exists() or not checksumValid(): _result = buildParser() _retcode = _result.returncode if _retcode != 0: From 0e8e936cdb9bf5e53483d75433a75eb8043988f4 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 8 Jul 2026 12:34:57 -0700 Subject: [PATCH 043/134] followFrom and followFromTrajectory now attempt to coerce pos to a vector. --- src/scenic/core/vectors.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index 912a79e36..83c640cfe 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -41,6 +41,7 @@ canCoerceType, coerceToFloat, toOrientation, + toVector, ) from scenic.core.utils import argsToString, cached_property @@ -718,7 +719,9 @@ def followFromTrajectory(self, pos, dist, steps=None, stepSize=None): stepSize (float): length used to compute how many steps to take, or :obj:`None` to use the field's default step size. """ - return self._followFromHelper(pos, dist, steps, stepSize, allPoints=True) + return self._followFromHelper( + toVector(pos), dist, steps, stepSize, allPoints=True + ) @vectorDistributionMethod def followFrom(self, pos, dist, steps=None, stepSize=None): @@ -736,7 +739,9 @@ def followFrom(self, pos, dist, steps=None, stepSize=None): stepSize (float): length used to compute how many steps to take, or :obj:`None` to use the field's default step size. """ - return self._followFromHelper(pos, dist, steps, stepSize, allPoints=False) + return self._followFromHelper( + toVector(pos), dist, steps, stepSize, allPoints=False + ) def _followFromHelper(self, pos, dist, steps, stepSize, allPoints): if allPoints: From b9b1570a27480f495d103a3c5ef65c778ef03fe5 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 8 Jul 2026 13:53:34 -0700 Subject: [PATCH 044/134] Added centerlines field to networks --- src/scenic/domains/driving/roads.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index 3fecfaf86..2d39994ab 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -888,6 +888,7 @@ class Network: sidewalkRegion: PolygonalRegion = None curbRegion: PolylineRegion = None shoulderRegion: PolygonalRegion = None + centerlines: PolylineRegion = None #: Traffic flow vector field aggregated over all roads (0 elsewhere). roadDirection: VectorField = None @@ -950,6 +951,11 @@ def __attrs_post_init__(self): edges.append(road.backwardLanes.curb) self.curbRegion = PolylineRegion.unionAll(edges) + if self.centerlines is None: + self.centerlines = PolylineRegion.unionAll( + [lane.centerline for lane in self.lanes] + ) + if self.roadDirection is None: # TODO replace with a PolygonalVectorField for better pruning self.roadDirection = VectorField("roadDirection", self._defaultRoadDirection) From 513299c4d066bad27d0e0c4a43270367411d2261 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 8 Jul 2026 13:54:20 -0700 Subject: [PATCH 045/134] Renamed centerlineRegion --- src/scenic/domains/driving/roads.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/scenic/domains/driving/roads.py b/src/scenic/domains/driving/roads.py index 2d39994ab..4c72ad7bd 100644 --- a/src/scenic/domains/driving/roads.py +++ b/src/scenic/domains/driving/roads.py @@ -888,7 +888,7 @@ class Network: sidewalkRegion: PolygonalRegion = None curbRegion: PolylineRegion = None shoulderRegion: PolygonalRegion = None - centerlines: PolylineRegion = None + centerlineRegion: PolylineRegion = None #: Traffic flow vector field aggregated over all roads (0 elsewhere). roadDirection: VectorField = None @@ -951,8 +951,8 @@ def __attrs_post_init__(self): edges.append(road.backwardLanes.curb) self.curbRegion = PolylineRegion.unionAll(edges) - if self.centerlines is None: - self.centerlines = PolylineRegion.unionAll( + if self.centerlineRegion is None: + self.centerlineRegion = PolylineRegion.unionAll( [lane.centerline for lane in self.lanes] ) From 6e536535b8510e9133ec20a2cc71add56e1e65f0 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 8 Jul 2026 15:37:32 -0700 Subject: [PATCH 046/134] Added additional tests. --- tests/syntax/test_compiler.py | 16 ++++++++++++++++ tests/syntax/test_parser.py | 27 +++++++++++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 597b85b55..e19c60dd4 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -2107,6 +2107,22 @@ def test_distance_to_op_from(self): case _: assert False + def test_min_distance_to_op(self): + node, _ = compileScenicAST(MinDistanceFromOp(Name("X"), None)) + match node: + case Call(Name("MinDistanceFrom"), [Name("X")], []): + assert True + case _: + assert False + + def test_min_distance_to_op_from(self): + node, _ = compileScenicAST(MinDistanceFromOp(Name("X"), Name("Y"))) + match node: + case Call(Name("MinDistanceFrom"), [Name("X")], [keyword("Y", Name("Y"))]): + assert True + case _: + assert False + def test_distance_past_op(self): node, _ = compileScenicAST(DistancePastOp(Name("X"))) match node: diff --git a/tests/syntax/test_parser.py b/tests/syntax/test_parser.py index b259b2ad0..e29662df2 100644 --- a/tests/syntax/test_parser.py +++ b/tests/syntax/test_parser.py @@ -2337,6 +2337,33 @@ def test_distance_to_from(self): case _: assert False + def test_min_distance_to(self): + mod = parse_string_helper("minimum distance to x") + stmt = mod.body[0] + match stmt: + case Expr(MinDistanceFromOp(Name("x"), None)): + assert True + case _: + assert False + + def test_min_distance_from_to(self): + mod = parse_string_helper("minimum distance from x to y") + stmt = mod.body[0] + match stmt: + case Expr(MinDistanceFromOp(Name("x"), Name("y"))): + assert True + case _: + assert False + + def test_min_distance_to_from(self): + mod = parse_string_helper("minimum distance to x from y") + stmt = mod.body[0] + match stmt: + case Expr(MinDistanceFromOp(Name("x"), Name("y"))): + assert True + case _: + assert False + @pytest.mark.parametrize( "code,expected", [ From 502ebab9f443b1ca20bf7e6447304b1c7c37265e Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 8 Jul 2026 17:19:27 -0700 Subject: [PATCH 047/134] Added tests for imported record, require monitor, and terminate statements. --- tests/syntax/helper_record.scenic | 1 + tests/syntax/helper_require_monitor.scenic | 5 +++ tests/syntax/helper_terminate.scenic | 1 + tests/syntax/test_imports.py | 46 +++++++++++++++++++++- 4 files changed, 52 insertions(+), 1 deletion(-) create mode 100644 tests/syntax/helper_record.scenic create mode 100644 tests/syntax/helper_require_monitor.scenic create mode 100644 tests/syntax/helper_terminate.scenic diff --git a/tests/syntax/helper_record.scenic b/tests/syntax/helper_record.scenic new file mode 100644 index 000000000..d77654391 --- /dev/null +++ b/tests/syntax/helper_record.scenic @@ -0,0 +1 @@ +record initial 1 as "foo" diff --git a/tests/syntax/helper_require_monitor.scenic b/tests/syntax/helper_require_monitor.scenic new file mode 100644 index 000000000..033f619dd --- /dev/null +++ b/tests/syntax/helper_require_monitor.scenic @@ -0,0 +1,5 @@ +monitor Bar(): + wait for 1 steps + terminate + +require monitor Bar() diff --git a/tests/syntax/helper_terminate.scenic b/tests/syntax/helper_terminate.scenic new file mode 100644 index 000000000..3e0d520d5 --- /dev/null +++ b/tests/syntax/helper_terminate.scenic @@ -0,0 +1 @@ +terminate after 1 seconds diff --git a/tests/syntax/test_imports.py b/tests/syntax/test_imports.py index c5b3ca0a5..a2ddbf666 100644 --- a/tests/syntax/test_imports.py +++ b/tests/syntax/test_imports.py @@ -13,7 +13,7 @@ from scenic import scenarioFromFile from scenic.core.errors import ScenicSyntaxError from scenic.syntax.translator import InvalidScenarioError -from tests.utils import compileScenic, sampleScene, sampleSceneFrom +from tests.utils import compileScenic, sampleResult, sampleScene, sampleSceneFrom def test_import_top_absolute(request): @@ -77,6 +77,50 @@ def test_inherit_requirements(runLocally): assert constrainedObj.position.x > 0 +def test_inherit_records(runLocally): + with runLocally(): + scenario = compileScenic( + """ + import helper_record + ego = new Object + """ + ) + + result = sampleResult(scenario, maxSteps=1) + assert "foo" in result.records + assert result.records["foo"] == 1 + + +def test_inherit_terminate(runLocally): + with runLocally(): + scenario = compileScenic( + """ + import helper_terminate + ego = new Object + record 1 as "foo" + """ + ) + + result = sampleResult(scenario, maxSteps=5) + assert "foo" in result.records + assert result.records["foo"] == [(0, 1)] + + +def test_inherit_require_monitor(runLocally): + with runLocally(): + scenario = compileScenic( + """ + import helper_require_monitor + ego = new Object + record 1 as "foo" + """ + ) + + result = sampleResult(scenario, maxSteps=5) + assert "foo" in result.records + assert result.records["foo"] == [(0, 1)] + + def test_inherit_constructors(runLocally): with runLocally(): scenario = compileScenic("from helper import Caerbannog\n" "ego = new Caerbannog") From f4987c08d6f9f3e6c2f4d33deff56127a0c5974c Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 9 Jul 2026 12:18:19 -0700 Subject: [PATCH 048/134] Partial fixes --- src/scenic/core/dynamics/scenarios.py | 8 ++++++++ src/scenic/domains/driving/behaviors.scenic | 2 +- tests/syntax/helper_terminate.scenic | 3 ++- tests/syntax/test_imports.py | 4 ++-- 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index 8aa1021b5..f6dc215e0 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -423,6 +423,14 @@ def _inherit(self, other): self._externalParameters.extend(other._externalParameters) self._requirements.extend(other._requirements) self._behaviors.extend(other._behaviors) + self._monitors.extend(other._monitors) + self._monitorRequirements.extend(other._monitorRequirements) + self._temporalRequirements.extend(other._temporalRequirements) + # self._terminationConditions.extend(other._terminationConditions) + # self._terminateSimulationConditions.extend(other._terminateSimulationConditions) + # self._recordedExprs.extend(other._recordedExprs) + # self._recordedInitialExprs.extend(other._recordedInitialExprs) + # self._recordedFinalExprs.extend(other._recordedFinalExprs) def _registerInstance(self, inst): self._instances.append(inst) diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors.scenic index 5db4794a6..757b031e0 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors.scenic @@ -13,7 +13,7 @@ from shapely.geometry import LineString, MultiPoint, Point as ShapelyPoint from scenic.core.regions import toPolygon from scenic.domains.driving.actions import * import scenic.domains.driving.model as _model -from scenic.domains.driving.roads import ManeuverType +from scenic.domains.driving.roads import ManeuverType, Lane behavior ConstantThrottleBehavior(x): diff --git a/tests/syntax/helper_terminate.scenic b/tests/syntax/helper_terminate.scenic index 3e0d520d5..afdfb691d 100644 --- a/tests/syntax/helper_terminate.scenic +++ b/tests/syntax/helper_terminate.scenic @@ -1 +1,2 @@ -terminate after 1 seconds +new Object +terminate after 1 steps diff --git a/tests/syntax/test_imports.py b/tests/syntax/test_imports.py index a2ddbf666..af519c9a6 100644 --- a/tests/syntax/test_imports.py +++ b/tests/syntax/test_imports.py @@ -103,7 +103,7 @@ def test_inherit_terminate(runLocally): result = sampleResult(scenario, maxSteps=5) assert "foo" in result.records - assert result.records["foo"] == [(0, 1)] + assert result.records["foo"] == [(0, 1), (1, 1)] def test_inherit_require_monitor(runLocally): @@ -118,7 +118,7 @@ def test_inherit_require_monitor(runLocally): result = sampleResult(scenario, maxSteps=5) assert "foo" in result.records - assert result.records["foo"] == [(0, 1)] + assert result.records["foo"] == [(0, 1), (1, 1)] def test_inherit_constructors(runLocally): From ef61631add85e6648a1d95980be0a49b5917bd79 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 10:35:07 -0700 Subject: [PATCH 049/134] Driving behaviors cleanup and work on behaviors for walking actors. --- src/scenic/core/dynamics/scenarios.py | 4 +- src/scenic/core/regions.py | 7 +- src/scenic/domains/driving/actions.py | 6 +- .../domains/driving/behaviors/__init__.py | 2 + .../steers.scenic} | 23 +-- .../domains/driving/behaviors/walks.scenic | 136 ++++++++++++++++++ src/scenic/simulators/metadrive/model.scenic | 3 + tests/syntax/helper_terminate.scenic | 1 - 8 files changed, 157 insertions(+), 25 deletions(-) create mode 100644 src/scenic/domains/driving/behaviors/__init__.py rename src/scenic/domains/driving/{behaviors.scenic => behaviors/steers.scenic} (94%) create mode 100644 src/scenic/domains/driving/behaviors/walks.scenic diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index f6dc215e0..af22fd990 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -426,8 +426,8 @@ def _inherit(self, other): self._monitors.extend(other._monitors) self._monitorRequirements.extend(other._monitorRequirements) self._temporalRequirements.extend(other._temporalRequirements) - # self._terminationConditions.extend(other._terminationConditions) - # self._terminateSimulationConditions.extend(other._terminateSimulationConditions) + self._terminationConditions.extend(other._terminationConditions) + self._terminateSimulationConditions.extend(other._terminateSimulationConditions) # self._recordedExprs.extend(other._recordedExprs) # self._recordedInitialExprs.extend(other._recordedInitialExprs) # self._recordedFinalExprs.extend(other._recordedFinalExprs) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 13afc1348..c8bf68057 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -57,7 +57,7 @@ triangulatePolygon, ) from scenic.core.lazy_eval import isLazy, valueInContext -from scenic.core.type_support import toOrientation, toScalar, toVector +from scenic.core.type_support import canCoerce, toOrientation, toScalar, toVector from scenic.core.utils import ( cached, cached_method, @@ -720,12 +720,17 @@ def toPolygon(thing): poly = thing.polygons elif hasattr(thing, "lineString"): poly = thing.lineString + elif isinstance(thing, Vector): + poly = shapely.Point(*thing) else: return None return poly +toShapely = toPolygon + + def regionFromShapelyObject(obj, orientation=None): """Build a 'Region' from Shapely geometry.""" assert obj.is_valid, obj diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index d8304ea9c..e32130ec6 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -56,12 +56,10 @@ class Walks: """ def setWalkingDirection(self, heading): - velocity = Vector(0, self.speed).rotatedBy(heading) - self.setVelocity(velocity) + self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): - velocity = speed * self.velocity.normalized() - self.setVelocity(velocity) + self.setVelocity(*Vector(0, speed).rotatedBy(self.heading)) ## Actions available to all agents diff --git a/src/scenic/domains/driving/behaviors/__init__.py b/src/scenic/domains/driving/behaviors/__init__.py new file mode 100644 index 000000000..9a5d0c77d --- /dev/null +++ b/src/scenic/domains/driving/behaviors/__init__.py @@ -0,0 +1,2 @@ +from scenic.domains.driving.behaviors.steers import * +from scenic.domains.driving.behaviors.walks import * diff --git a/src/scenic/domains/driving/behaviors.scenic b/src/scenic/domains/driving/behaviors/steers.scenic similarity index 94% rename from src/scenic/domains/driving/behaviors.scenic rename to src/scenic/domains/driving/behaviors/steers.scenic index 757b031e0..e64aca8ef 100644 --- a/src/scenic/domains/driving/behaviors.scenic +++ b/src/scenic/domains/driving/behaviors/steers.scenic @@ -8,14 +8,14 @@ from abc import ABC, abstractmethod import warnings import shapely -from shapely.geometry import LineString, MultiPoint, Point as ShapelyPoint +from shapely.geometry import GeometryCollection, MultiPolygon, Polygon, MultiLineString, LineString, MultiPoint, Point as ShapelyPoint -from scenic.core.regions import toPolygon +from scenic.core.regions import toShapely +from scenic.core.type_support import toVector from scenic.domains.driving.actions import * import scenic.domains.driving.model as _model from scenic.domains.driving.roads import ManeuverType, Lane - behavior ConstantThrottleBehavior(x): while True: take SetThrottleAction(x), SetReverseAction(False), SetHandBrakeAction(False) @@ -29,17 +29,6 @@ behavior DriveAvoidingCollisions(target_speed=25, avoidance_threshold=10): behavior AccelerateForwardBehavior(): take SetReverseAction(False), SetHandBrakeAction(False), SetThrottleAction(0.5) -behavior WalkForwardBehavior(): - """Walk forward behavior for pedestrians. - - It will uniformly randomly choose either end of the sidewalk that the pedestrian is on, and have the pedestrian walk towards the endpoint. - """ - current_sidewalk = _model.network.sidewalkAt(self.position) - end_point = Uniform(*current_sidewalk.centerline.points) - end_vec = end_point[0] @ end_point[1] - normal_vec = Vector.normalized(end_vec) - take WalkTowardsAction(goal_position=normal_vec), SetSpeedAction(speed=1) - behavior ConstantThrottleBehavior(x): take SetThrottleAction(x) @@ -143,13 +132,13 @@ class Trajectory(object): return self.polyline.length def getRelativeTime(self, pos): - return toPolygon(self.polyline).project(ShapelyPoint(*pos), normalized=True)*self.duration + return toShapely(self.polyline).project(ShapelyPoint(*pos), normalized=True)*self.duration def getTimedDistance(self, timeA, timeB): - return shapely.ops.substring(toPolygon(self.polyline), timeA/self.duration, timeB/self.duration, normalized=True).length + return shapely.ops.substring(toShapely(self.polyline), timeA/self.duration, timeB/self.duration, normalized=True).length def __getitem__(self, time): - pt = toPolygon(self.polyline).interpolate(time/self.duration, normalized=True) + pt = toShapely(self.polyline).interpolate(time/self.duration, normalized=True) return Vector(pt.x, pt.y) @staticmethod diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic new file mode 100644 index 000000000..dfbff3e73 --- /dev/null +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -0,0 +1,136 @@ +## Pedestrian Behaviors +def _getBugPath(self, target, backgroundObjects, bufferConst=1): + """ Generate a walking path using a Bug algorithm approach.""" + # Compute the buffer amount based on our bounding radius and bufferConst + buffer_amount = shapely.minimum_bounding_radius(self._boundingPolygon) + bufferConst + + # Generate the raw path, from the current position straight towards the target. + path = LineString([toShapely(self.position), toShapely(toVector(target))]) + + # Compute the obstacle polygons + obstacle_multi_poly = shapely.union_all(list(obj._boundingPolygon.buffer(buffer_amount) for obj in backgroundObjects)) + if isinstance(obstacle_multi_poly, MultiPolygon): + obstacle_polys = obstacle_multi_poly.geoms + assert all(isinstance(geom, Polygon) for geom in obstacle_polys) + elif isinstance(obstacle_multi_poly, Polygon): + obstacle_polys = [obstacle_multi_poly] + else: + assert False + + # Refine path around obstacles, going from those with the largest boundary inwards + # (to account for the rare case where an obstacle poly may be entirely contained in another) + for obstacle_poly in sorted(obstacle_polys, key=lambda x: x.boundary.length, reverse=True): + self_pt = ShapelyPoint(self.position) + target_pt = ShapelyPoint(path.coords[-1]) + if obstacle_poly.contains(target_pt): + # Check if target is inside the polygon. + # If we're too close to the exterior point, return None. + if obstacle_poly.distance(self_pt) < 0.01: + return None + + # Otherwise, truncate the path to the closest point on the exterior of the obstacle_poly. + exterior_intersection = path.intersection(obstacle_poly.exterior) + stop_pt = shapely.ops.nearest_points(exterior_intersection, self_pt)[0] + path = shapely.ops.substring(path, 0, path.project(stop_pt, normalized=True), normalized=True) + continue + + if path.intersects(obstacle_poly): + # Find intersection points of path with exterior, and extract the first and + # last with respect to their distance along the path. + exterior_intersection = path.intersection(obstacle_poly.exterior) + intersection_points = [] + + # If we're inside the obstacle poly, add the closest exterior point to guide us out. + if obstacle_poly.contains(self_pt): + intersection_points.append(path.interpolate(path.project(self_pt))) + + if isinstance(exterior_intersection, ShapelyPoint): + assert obstacle_poly.contains(self_pt) + intersection_points.append(exterior_intersection) + elif isinstance(exterior_intersection, LineString): + instersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom,coords[1])] + elif isinstance(exterior_intersection, (MultiPoint, MultiLineString, GeometryCollection)): + for geom in exterior_intersection.geoms: + if isinstance(geom, ShapelyPoint): + intersection_points.append(geom) + elif isinstance(geom, LineString): + instersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom,coords[1])] + else: + assert False + + intersection_points.sort(key=lambda x: path.project(x)) + start_pt = intersection_points[0] + end_pt = intersection_points[-1] + + # Split the exterior ring into two segments at these points + # split_line = shapely.affinity.scale(LineString([start_pt, end_pt]), 2, 2) + exterior_ls = LineString(obstacle_poly.exterior) + start_pt_s = exterior_ls.project(start_pt, normalized=True) + exterior_ls = LineString(list(shapely.ops.substring(exterior_ls, start_pt_s, 1, normalized=True).coords) + + list(shapely.ops.substring(exterior_ls, 0, start_pt_s, normalized=True).coords)) + end_pt_ls = exterior_ls.project(end_pt, normalized=True) + exterior_segments = [ + shapely.ops.substring(exterior_ls, 0, end_pt_ls, normalized=True), + shapely.ops.substring(exterior_ls, end_pt_ls, 1, normalized=True) + ] + + # Patch together the shorter of these exterior segments with the path. + start_path = shapely.ops.substring(path, 0, path.project(start_pt, normalized=True), normalized=True) + end_path = shapely.ops.substring(path, path.project(end_pt, normalized=True), 1, normalized=True) + start_path = shapely.force_2d(start_path) + end_path = shapely.force_2d(end_path) + + # Extract and reverse mid_path if needed + mid_path = sorted(exterior_segments, key=lambda x: x.length)[0] + mid_path_start = ShapelyPoint(mid_path.coords[0]) + if (ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(start_path.coords[0])) + > ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(end_path.coords[0]))): + mid_path = mid_path.reverse() + + path = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) + + return path + +behavior WalkPath(path, targetSpeed, terminationThresh=0.1, replanTime=0.5): + """ Walk a path at targetSpeed, stopping at the end.""" + if not isinstance(path, PolylineRegion): + raise ValueError("`path` must be a `PolylineRegion`.") + path = path.lineString + + while distance from self to target > doneThresh: + # Find where the actor currently is on the path + start_s = path.project(ShapelyPoint(self.position)) + path_distance = path.interpolate(start_s).distance(ShapelyPoint(self.position)) + + # Compute lookahead distance from speed and timestep. This accounts for how + # far we are from the path as well, so that we prioritize returning to it. + lookahead_dist = max(targetSpeed * simulation().timestep - path_distance, 0) + + # Find target point + target_point = Vector(*path.interpolate(start_s+lookahead_dist).coords[0]) + + # Set appropriate heading and velocity, calculating actual speed we should aim + # for, so we don't overshoot if we cut a corner or are at the end of the path. + actual_speed = min(targetSpeed, (distance from self to target_point)/simulation().timestep) + heading = angle from self to target_point + take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) + + take SetWalkingSpeedAction(0) + +behavior WalkTo(target, targetSpeed, terminationThresh=0.1, replanTime=0.5): + """ Walk towards a given target position at targetSpeed, stopping at the end.""" + path = PolylineRegion(points=(self.position, targetSpeed)) + do WalkPath(path) + +# TODO: This uses WalkTowardsAction which doesn't exist. +behavior WalkForwardBehavior(): + """Walk forward behavior for pedestrians. + + It will uniformly randomly choose either end of the sidewalk that + the pedestrian is on, and have the pedestrian walk towards the endpoint. + """ + current_sidewalk = _model.network.sidewalkAt(self.position) + end_point = Uniform(*current_sidewalk.centerline.points) + end_vec = end_point[0] @ end_point[1] + normal_vec = Vector.normalized(end_vec) + take WalkTowardsAction(goal_position=normal_vec), SetSpeedAction(speed=1) diff --git a/src/scenic/simulators/metadrive/model.scenic b/src/scenic/simulators/metadrive/model.scenic index 078c6fc33..145795004 100644 --- a/src/scenic/simulators/metadrive/model.scenic +++ b/src/scenic/simulators/metadrive/model.scenic @@ -125,6 +125,9 @@ class MetaDriveActor(DrivingObject): def setVelocity(self, vel): self.metaDriveActor.set_velocity(vel) + def setOrientation(self, orientation): + converted_heading = scenicToMetaDriveHeading(orientation.yaw) + self.metaDriveActor.set_heading_theta(converted_heading) class Vehicle(Vehicle, Steers, MetaDriveActor): def __init__(self, *args, **kwargs): diff --git a/tests/syntax/helper_terminate.scenic b/tests/syntax/helper_terminate.scenic index afdfb691d..62fa24799 100644 --- a/tests/syntax/helper_terminate.scenic +++ b/tests/syntax/helper_terminate.scenic @@ -1,2 +1 @@ -new Object terminate after 1 steps From 1a129a149c292b246bc4720f5069c024ebdeed13 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 14:55:07 -0700 Subject: [PATCH 050/134] Working obstacle avoidance in walking behavior. --- src/scenic/core/regions.py | 5 + .../domains/driving/behaviors/walks.scenic | 155 +++++++++++++----- src/scenic/simulators/metadrive/simulator.py | 1 + src/scenic/simulators/newtonian/simulator.py | 1 + 4 files changed, 120 insertions(+), 42 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index c8bf68057..2544f9a77 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -3618,6 +3618,11 @@ def end(self): roll=orientation.roll, ) + @cached + def reverse(self): + """Return a copy of this `PolylineRegion`, reversed.""" + return PolylineRegion(polyline=self.lineString.reverse()) + def defaultOrientation(self, point): start, end = self.nearestSegmentTo(point) return start.angleTo(end) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index dfbff3e73..4bee5420e 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -1,11 +1,19 @@ +import shapely +from shapely.geometry import GeometryCollection, MultiPolygon, Polygon, MultiLineString, LineString, MultiPoint, Point as ShapelyPoint + +import scenic.domains.driving.model as _model +from scenic.core.regions import toShapely +from scenic.core.type_support import toVector +from scenic.domains.driving.actions import * + + ## Pedestrian Behaviors -def _getBugPath(self, target, backgroundObjects, bufferConst=1): - """ Generate a walking path using a Bug algorithm approach.""" - # Compute the buffer amount based on our bounding radius and bufferConst - buffer_amount = shapely.minimum_bounding_radius(self._boundingPolygon) + bufferConst +def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): + """ Refine a walking path using a Bug algorithm approach.""" + assert isinstance(path_ls, LineString) - # Generate the raw path, from the current position straight towards the target. - path = LineString([toShapely(self.position), toShapely(toVector(target))]) + # Compute the buffer amount based on our bounding radius and bufferConst + buffer_amount = shapely.minimum_bounding_radius(actor._boundingPolygon) + bufferConst # Compute the obstacle polygons obstacle_multi_poly = shapely.union_all(list(obj._boundingPolygon.buffer(buffer_amount) for obj in backgroundObjects)) @@ -20,8 +28,8 @@ def _getBugPath(self, target, backgroundObjects, bufferConst=1): # Refine path around obstacles, going from those with the largest boundary inwards # (to account for the rare case where an obstacle poly may be entirely contained in another) for obstacle_poly in sorted(obstacle_polys, key=lambda x: x.boundary.length, reverse=True): - self_pt = ShapelyPoint(self.position) - target_pt = ShapelyPoint(path.coords[-1]) + self_pt = ShapelyPoint(actor.position) + target_pt = ShapelyPoint(path_ls.coords[-1]) if obstacle_poly.contains(target_pt): # Check if target is inside the polygon. # If we're too close to the exterior point, return None. @@ -29,23 +37,23 @@ def _getBugPath(self, target, backgroundObjects, bufferConst=1): return None # Otherwise, truncate the path to the closest point on the exterior of the obstacle_poly. - exterior_intersection = path.intersection(obstacle_poly.exterior) + exterior_intersection = path_ls.intersection(obstacle_poly.exterior) stop_pt = shapely.ops.nearest_points(exterior_intersection, self_pt)[0] - path = shapely.ops.substring(path, 0, path.project(stop_pt, normalized=True), normalized=True) + path_ls = shapely.ops.substring(path_ls, 0, path_ls.project(stop_pt, normalized=True), normalized=True) continue - if path.intersects(obstacle_poly): + if path_ls.intersects(obstacle_poly): # Find intersection points of path with exterior, and extract the first and # last with respect to their distance along the path. - exterior_intersection = path.intersection(obstacle_poly.exterior) + exterior_intersection = path_ls.intersection(obstacle_poly.exterior) intersection_points = [] # If we're inside the obstacle poly, add the closest exterior point to guide us out. if obstacle_poly.contains(self_pt): - intersection_points.append(path.interpolate(path.project(self_pt))) + intersection_points.append(path_ls.interpolate(path_ls.project(self_pt))) if isinstance(exterior_intersection, ShapelyPoint): - assert obstacle_poly.contains(self_pt) + assert obstacle_poly.contains(ShapelyPoint(path_ls.coords[0])) intersection_points.append(exterior_intersection) elif isinstance(exterior_intersection, LineString): instersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom,coords[1])] @@ -58,7 +66,7 @@ def _getBugPath(self, target, backgroundObjects, bufferConst=1): else: assert False - intersection_points.sort(key=lambda x: path.project(x)) + intersection_points.sort(key=lambda x: path_ls.project(x)) start_pt = intersection_points[0] end_pt = intersection_points[-1] @@ -75,8 +83,8 @@ def _getBugPath(self, target, backgroundObjects, bufferConst=1): ] # Patch together the shorter of these exterior segments with the path. - start_path = shapely.ops.substring(path, 0, path.project(start_pt, normalized=True), normalized=True) - end_path = shapely.ops.substring(path, path.project(end_pt, normalized=True), 1, normalized=True) + start_path = shapely.ops.substring(path_ls, 0, path_ls.project(start_pt, normalized=True), normalized=True) + end_path = shapely.ops.substring(path_ls, path_ls.project(end_pt, normalized=True), 1, normalized=True) start_path = shapely.force_2d(start_path) end_path = shapely.force_2d(end_path) @@ -87,40 +95,103 @@ def _getBugPath(self, target, backgroundObjects, bufferConst=1): > ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(end_path.coords[0]))): mid_path = mid_path.reverse() - path = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) + path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) - return path + return path_ls -behavior WalkPath(path, targetSpeed, terminationThresh=0.1, replanTime=0.5): +behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.5, bufferConst=1): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") - path = path.lineString - - while distance from self to target > doneThresh: - # Find where the actor currently is on the path - start_s = path.project(ShapelyPoint(self.position)) - path_distance = path.interpolate(start_s).distance(ShapelyPoint(self.position)) - - # Compute lookahead distance from speed and timestep. This accounts for how - # far we are from the path as well, so that we prioritize returning to it. - lookahead_dist = max(targetSpeed * simulation().timestep - path_distance, 0) - - # Find target point - target_point = Vector(*path.interpolate(start_s+lookahead_dist).coords[0]) - - # Set appropriate heading and velocity, calculating actual speed we should aim - # for, so we don't overshoot if we cut a corner or are at the end of the path. - actual_speed = min(targetSpeed, (distance from self to target_point)/simulation().timestep) - heading = angle from self to target_point - take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) + path_ls = path.lineString + + while distance from self to path.end > terminationThresh: + # Refine the path by dropping already traversed areas and adding a link to the start. + start_s = path_ls.project(ShapelyPoint(self.position), normalized=True) + refined_path_ls = shapely.ops.substring(path_ls, start_s, 1, normalized=True) + refined_path_ls = LineString([self.position] + list(refined_path_ls.coords)) + + # If our immediate path has us cross through any objects in motion, stop and + # wait until it's clear. + background_objects = [obj for obj in simulation().objects if obj is not self] + immediate_path = shapely.ops.substring(refined_path_ls, 0, targetSpeed) + danger_objects = [obj for obj in background_objects if obj.speed > 0.1] + buffer_amount = shapely.minimum_bounding_radius(self._boundingPolygon) + bufferConst + moving_obj_danger_zone = shapely.union_all( + list(obj._boundingPolygon.buffer(buffer_amount) for obj in danger_objects) + ) + if immediate_path.intersects(moving_obj_danger_zone): + # Tie-breaking wait + take SetWalkingSpeedAction(0) + wait for DiscreteRange(1, 5) steps + else: + # Modify path to route around objects. + refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst) + + # Find where the actor currently is on the refined path + start_s = refined_path_ls.project(ShapelyPoint(self.position)) + path_distance = refined_path_ls.interpolate(start_s).distance(ShapelyPoint(self.position)) + + # Compute lookahead distance from speed and timestep. This accounts for how + # far we are from the path as well, so that we prioritize returning to it. + lookahead_dist = max(targetSpeed * simulation().timestep - path_distance, 0) + + # Find target point + target_point = Vector(*refined_path_ls.interpolate(start_s+lookahead_dist).coords[0]) + + # Set appropriate heading and velocity, calculating actual speed we should aim + # for, so we don't overshoot if we cut a corner or are at the end of the path. + actual_speed = min(targetSpeed, (distance from self to target_point)/simulation().timestep) + heading = angle from self to target_point + take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) take SetWalkingSpeedAction(0) -behavior WalkTo(target, targetSpeed, terminationThresh=0.1, replanTime=0.5): +behavior WalkTo(target, targetSpeed, *, avoidObstacles=True): """ Walk towards a given target position at targetSpeed, stopping at the end.""" - path = PolylineRegion(points=(self.position, targetSpeed)) - do WalkPath(path) + path = PolylineRegion(points=(self.position, toVector(target))) + do WalkPath(path, targetSpeed, avoidObstacles=avoidObstacles) + +behavior Walk(targetSpeed=None, backwards=None, avoidObstacles=True): + if targetSpeed is None: + # TODO: Should we move this to a property of pedestrians? (`baseWalkSpeed`?) + targetSpeed = Range(0.9, 1.8) # From ~2mph to ~4mph + + if backwards is None: + backwards = Uniform(True, False) + + network = _model.network + + while True: + # If we're not currently in a walkable region, return to the closest one. + if self.position not in network.walkableRegion: + # TODO: Replace with closest point in region operator. + closest_pt = shapely.ops.nearest_points(toShapely(network.walkableRegion), toShapely(self.position))[0] + target_element = network.findPointIn(Vector(*closest_pt.coords[0]), network.sidewalks+network.crossings, reject=False) + target_pt = target_element.centerline.project(self.position) + do WalkTo(target_pt, targetSpeed=targetSpeed, avoidObstacles=avoidObstacles) + continue + + # If we're not close to the start or end of the centerline of our current element + # (depending on whether we are walking `backwards` or not), walk towards it following the centerline. + current_element = network.findPointIn(self.position, network.sidewalks+network.crossings, reject=False) + end_pt = current_element.centerline.start if backwards else current_element.centerline.end + if distance from self.position to end_pt > 0.1: + target_path = current_element.centerline.reverse() if backwards else current_element.centerline + do WalkPath(target_path, targetSpeed=targetSpeed, avoidObstacles=avoidObstacles) + continue + + # If we're at the end of the current element, we should pick a successor/predecessor + # (depending on whether we are walking `backwards`). + # TODO: Randomly pick from ALL successors/predecessors and sidewalks. + next_element = current_element._predecessor if backwards else current_element._successor + if next_element is not None: + target_path = next_element.centerline.reverse() if backwards else next_element.centerline + do WalkPath(target_path, targetSpeed=targetSpeed, avoidObstacles=avoidObstacles) + continue + + # We have no valid next moves. Terminate the behavior. + return # TODO: This uses WalkTowardsAction which doesn't exist. behavior WalkForwardBehavior(): diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 075134f55..7410c7345 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -293,6 +293,7 @@ def executeActions(self, allActions): math.cos(obj._walking_direction), math.sin(obj._walking_direction), ] + obj.setOrientation(toOrientation(obj.heading)) obj.metaDriveActor.set_velocity(direction, obj._walking_speed) def step(self): diff --git a/src/scenic/simulators/newtonian/simulator.py b/src/scenic/simulators/newtonian/simulator.py index 4c23adaec..b9945e145 100644 --- a/src/scenic/simulators/newtonian/simulator.py +++ b/src/scenic/simulators/newtonian/simulator.py @@ -215,6 +215,7 @@ def step(self): else obj.speed ) obj.velocity = Vector(0, s).rotatedBy(h) + obj.speed = obj.velocity.norm() # 2) Vehicle: throttle/brake/steer physics elif getattr(obj, "isCar", False): forward = obj.velocity.dot(Vector(0, 1).rotatedBy(obj.heading)) >= 0 From 0215e8163479b9b555ca83e4b3d7ecf5f6321e44 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 16:47:56 -0700 Subject: [PATCH 051/134] Pedestrian WalkTo polish. --- .../domains/driving/behaviors/walks.scenic | 82 +++++++++++++------ tests/simulators/newtonian/test_newtonian.py | 40 +++++++++ 2 files changed, 99 insertions(+), 23 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 4bee5420e..d3f52f678 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -99,6 +99,13 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): return path_ls +behavior TieBreakingPause(): + take SetWalkingSpeedAction(0) + wait for Range(0.1, 0.5) seconds + +import matplotlib.pyplot as plt +from scenic.core.geometry import plotPolygon + behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.5, bufferConst=1): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): @@ -121,30 +128,59 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 list(obj._boundingPolygon.buffer(buffer_amount) for obj in danger_objects) ) if immediate_path.intersects(moving_obj_danger_zone): - # Tie-breaking wait - take SetWalkingSpeedAction(0) - wait for DiscreteRange(1, 5) steps + do TieBreakingPause() + continue + + # Modify path to route around objects. + old_refined_path_ls = refined_path_ls + refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst) + # If refined_path_ls is None, our goal is inside the danger zone and we can't + # proceed further right now. + if refined_path_ls is None: + do TieBreakingPause() + continue + + # Find where the actor currently is on the refined path + start_s = refined_path_ls.project(ShapelyPoint(self.position)) + + # Compute lookahead distance from speed and timestep. + # Look ahead two timesteps to avoid ping-ponging. + lookahead_dist = targetSpeed * 2*simulation().timestep + + # Find target point + lookahead_circle = toShapely( + CircularRegion(self.position, lookahead_dist) + ) + intersection_geometry = lookahead_circle.boundary.intersection(refined_path_ls) + + if intersection_geometry.is_empty: + # No viable target points. If we're close enough to the end of the path, aim for that. + # Otherwise, aim for the closest point on the trajectory. + end_pt = Vector(*refined_path_ls.coords[-1]) + if distance from self.position to end_pt <= lookahead_dist: + target_point = end_pt + else: + target_point = Vector(*shapely.ops.nearest_points(refined_path_ls, ShapelyPoint(self.position))[0]) + elif isinstance(intersection_geometry, ShapelyPoint): + target_point = Vector(*intersection_geometry.coords[0]) + elif isinstance(intersection_geometry, MultiPoint): + # There are multiple candidate target points. Pick the one that appears last on the path. + # This helps us progress, and we don't have to worry about skipping significant parts of the path + # with such a small lookahead distance. + target_point = sorted( + intersection_geometry.geoms, key=lambda pt: refined_path_ls.project(pt) + )[-1] + target_point = Vector(*target_point.coords[0]) else: - # Modify path to route around objects. - refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst) - - # Find where the actor currently is on the refined path - start_s = refined_path_ls.project(ShapelyPoint(self.position)) - path_distance = refined_path_ls.interpolate(start_s).distance(ShapelyPoint(self.position)) - - # Compute lookahead distance from speed and timestep. This accounts for how - # far we are from the path as well, so that we prioritize returning to it. - lookahead_dist = max(targetSpeed * simulation().timestep - path_distance, 0) - - # Find target point - target_point = Vector(*refined_path_ls.interpolate(start_s+lookahead_dist).coords[0]) - - # Set appropriate heading and velocity, calculating actual speed we should aim - # for, so we don't overshoot if we cut a corner or are at the end of the path. - actual_speed = min(targetSpeed, (distance from self to target_point)/simulation().timestep) - heading = angle from self to target_point - take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) - + # We've gotten something strange. Fall back to a representative point as the target. + target_point = Vector(*intersection_geometry.representative_point().coords[0]) + + # Set appropriate heading and velocity, calculating actual speed we should aim + # for, so we don't overshoot if we cut a corner or are at the end of the path. + actual_speed = min(targetSpeed, (distance from self to target_point)/simulation().timestep) + heading = angle from self to target_point + take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) + take SetWalkingSpeedAction(0) behavior WalkTo(target, targetSpeed, *, avoidObstacles=True): diff --git a/tests/simulators/newtonian/test_newtonian.py b/tests/simulators/newtonian/test_newtonian.py index 98561c527..578f82b6c 100644 --- a/tests/simulators/newtonian/test_newtonian.py +++ b/tests/simulators/newtonian/test_newtonian.py @@ -4,6 +4,7 @@ from PIL import Image as IPImage import pytest +from scenic.core.simulators import TerminationType from scenic.domains.driving.roads import Network from scenic.simulators.newtonian import NewtonianSimulator from tests.utils import compileScenic, pickle_test, sampleScene, tryPickling @@ -113,3 +114,42 @@ def test_pedestrian_velocity_vector(getAssetPath): # Expect movement northeast (positive dx and dy) assert dx > 0.1, f"Expected positive x movement (east), got dx = {dx}" assert dy > 0.1, f"Expected positive y movement (north), got dy = {dy}" + + +## Pedestrian Behaviors +@pytest.mark.slow +def test_pedestrian_sidewalk_conflict(getAssetPath): + mapPath = getAssetPath("maps/CARLA/Town01.xodr") + + code = f""" + param map = r'{mapPath}' + model scenic.simulators.newtonian.driving_model + + targetSidewalk = Uniform(*network.sidewalks) + + pedASpeed = Range(0.9, 1.8) + pedBSpeed = Range(0.9, 1.8) + + pedA = new Pedestrian at targetSidewalk.centerline.start, + with behavior WalkTo(targetSidewalk.centerline.end, targetSpeed=pedASpeed) + + pedB = new Pedestrian at targetSidewalk.centerline.end, + with behavior WalkTo(targetSidewalk.centerline.start, targetSpeed=pedBSpeed) + + param minPedSpeed = min(pedASpeed, pedBSpeed) + param pedDistance = targetSidewalk.centerline.length + + require targetSidewalk.centerline.length >= 10 + + terminate when ((distance from pedA to targetSidewalk.centerline.end) < 0.1 + and (distance from pedB to targetSidewalk.centerline.start) < 0.1) + """ + scenario = compileScenic(code, mode2D=True) + for _ in range(5): + scene, _ = scenario.generate(maxIterations=100) + simulator = NewtonianSimulator(render=False) + maxSteps = int( + 2 * (scene.params["pedDistance"] / scene.params["minPedSpeed"]) / 0.1 + ) + simulation = simulator.simulate(scene, maxSteps=maxSteps) + assert simulation.result.terminationType == TerminationType.scenarioComplete From bcb7d9836bb3a0c4c3e33679de2f0f02a7a2debe Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:01:26 -0700 Subject: [PATCH 052/134] Fixed setWalkingSpeed. --- src/scenic/domains/driving/actions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index e32130ec6..6d61c27db 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -59,7 +59,7 @@ def setWalkingDirection(self, heading): self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): - self.setVelocity(*Vector(0, speed).rotatedBy(self.heading)) + self.setVelocity(Vector(0, speed).rotatedBy(self.heading)) ## Actions available to all agents From b36fd584dc142afa00be21ad16433246b2256612 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:13:00 -0700 Subject: [PATCH 053/134] Clean lines in WalkTo --- src/scenic/domains/driving/behaviors/walks.scenic | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index d3f52f678..497d6e0a2 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -117,6 +117,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 start_s = path_ls.project(ShapelyPoint(self.position), normalized=True) refined_path_ls = shapely.ops.substring(path_ls, start_s, 1, normalized=True) refined_path_ls = LineString([self.position] + list(refined_path_ls.coords)) + refined_path_ls = shapely.remove_repeated_points(refined_path_ls) # If our immediate path has us cross through any objects in motion, stop and # wait until it's clear. @@ -132,8 +133,8 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 continue # Modify path to route around objects. - old_refined_path_ls = refined_path_ls refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst) + refined_path_ls = shapely.remove_repeated_points(refined_path_ls) # If refined_path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. if refined_path_ls is None: From ac71f711891fa91940d7760b57d0ff07c997ca1d Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:14:02 -0700 Subject: [PATCH 054/134] More line cleaning. --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 497d6e0a2..98121f8be 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -96,6 +96,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): mid_path = mid_path.reverse() path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) + path_ls = shapely.remove_repeated_points(path_ls) return path_ls From c7a098328f25d12afce64c8e66c56a7f2cae4824 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:17:26 -0700 Subject: [PATCH 055/134] More cleaning. --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 98121f8be..dc372ca1b 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -73,6 +73,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): # Split the exterior ring into two segments at these points # split_line = shapely.affinity.scale(LineString([start_pt, end_pt]), 2, 2) exterior_ls = LineString(obstacle_poly.exterior) + exterior_ls = shapely.remove_repeated_points(exterior_ls) start_pt_s = exterior_ls.project(start_pt, normalized=True) exterior_ls = LineString(list(shapely.ops.substring(exterior_ls, start_pt_s, 1, normalized=True).coords) + list(shapely.ops.substring(exterior_ls, 0, start_pt_s, normalized=True).coords)) From 51594ecb6af43e1fe0d80cd6998bf117e00cafc9 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:20:21 -0700 Subject: [PATCH 056/134] More cleanup --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index dc372ca1b..429fe33e8 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -77,6 +77,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): start_pt_s = exterior_ls.project(start_pt, normalized=True) exterior_ls = LineString(list(shapely.ops.substring(exterior_ls, start_pt_s, 1, normalized=True).coords) + list(shapely.ops.substring(exterior_ls, 0, start_pt_s, normalized=True).coords)) + exterior_ls = shapely.remove_repeated_points(exterior_ls) end_pt_ls = exterior_ls.project(end_pt, normalized=True) exterior_segments = [ shapely.ops.substring(exterior_ls, 0, end_pt_ls, normalized=True), From d1f42881e1dc583ad28b79e48e055f600cb27f37 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:20:46 -0700 Subject: [PATCH 057/134] Additional cleanup --- src/scenic/domains/driving/behaviors/walks.scenic | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 429fe33e8..c5eea627b 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -71,9 +71,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): end_pt = intersection_points[-1] # Split the exterior ring into two segments at these points - # split_line = shapely.affinity.scale(LineString([start_pt, end_pt]), 2, 2) exterior_ls = LineString(obstacle_poly.exterior) - exterior_ls = shapely.remove_repeated_points(exterior_ls) start_pt_s = exterior_ls.project(start_pt, normalized=True) exterior_ls = LineString(list(shapely.ops.substring(exterior_ls, start_pt_s, 1, normalized=True).coords) + list(shapely.ops.substring(exterior_ls, 0, start_pt_s, normalized=True).coords)) From e2d44f2ba069c33c23a06162040fa596c67e2372 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 10 Jul 2026 17:21:43 -0700 Subject: [PATCH 058/134] Cleanup. --- src/scenic/domains/driving/behaviors/walks.scenic | 1 - 1 file changed, 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index c5eea627b..0a196decd 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -135,7 +135,6 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 # Modify path to route around objects. refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst) - refined_path_ls = shapely.remove_repeated_points(refined_path_ls) # If refined_path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. if refined_path_ls is None: From ddee66948c3412d01078c1c41fda3fb7beec703d Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 14:44:44 -0700 Subject: [PATCH 059/134] Added polyline level substring method. --- src/scenic/core/regions.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 2544f9a77..0e76fc169 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -3751,6 +3751,18 @@ def pointAlongBy(self, distance, normalized=False) -> Vector: pt = self.lineString.interpolate(distance, normalized=normalized) return Vector(pt.x, pt.y) + def substring(self, start, end, normalized=False): + """Compute a substring of this polyline. + + If **normalized** is true, then start and end should be between 0 and 1, and + are interpreted as a fraction of the length of the polyline. + """ + return PolylineRegion( + polyline=shapely.ops.substring( + self.lineString, start, end, normalized=normalized + ) + ) + def equallySpacedPoints(self, num): return [self.pointAlongBy(d) for d in numpy.linspace(0, self.length, num)] From d990704942549f12892ffae088506fb3a5b22365 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 15:20:40 -0700 Subject: [PATCH 060/134] Improved pedestrian walking behavior. --- .../domains/driving/behaviors/walks.scenic | 60 ++++++------------- 1 file changed, 18 insertions(+), 42 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 0a196decd..3ef47d9c8 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -104,8 +104,19 @@ behavior TieBreakingPause(): take SetWalkingSpeedAction(0) wait for Range(0.1, 0.5) seconds -import matplotlib.pyplot as plt -from scenic.core.geometry import plotPolygon +behavior _WalkPathHelper(path, targetSpeed): + # Start distAlong negative to ensure we can get back to the path if we somehow + # start away from it. + dist_along = -(distance from self to Vector(*path.coords[0])) + while True: + # Determine target point, which will move along the path until we re-plan. + dist_along += targetSpeed + target_pt = Vector(*path.interpolate(max(0, dist_along)).coords[0]) + # Set appropriate heading and velocity, calculating actual speed we should aim + # for, so we don't overshoot if we cut a corner or are at the end of the path. + actual_speed = min(targetSpeed, (distance from self to target_pt)/simulation().timestep) + heading = angle from self to target_pt + take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.5, bufferConst=1): """ Walk a path at targetSpeed, stopping at the end.""" @@ -141,46 +152,11 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 do TieBreakingPause() continue - # Find where the actor currently is on the refined path - start_s = refined_path_ls.project(ShapelyPoint(self.position)) - - # Compute lookahead distance from speed and timestep. - # Look ahead two timesteps to avoid ping-ponging. - lookahead_dist = targetSpeed * 2*simulation().timestep - - # Find target point - lookahead_circle = toShapely( - CircularRegion(self.position, lookahead_dist) - ) - intersection_geometry = lookahead_circle.boundary.intersection(refined_path_ls) - - if intersection_geometry.is_empty: - # No viable target points. If we're close enough to the end of the path, aim for that. - # Otherwise, aim for the closest point on the trajectory. - end_pt = Vector(*refined_path_ls.coords[-1]) - if distance from self.position to end_pt <= lookahead_dist: - target_point = end_pt - else: - target_point = Vector(*shapely.ops.nearest_points(refined_path_ls, ShapelyPoint(self.position))[0]) - elif isinstance(intersection_geometry, ShapelyPoint): - target_point = Vector(*intersection_geometry.coords[0]) - elif isinstance(intersection_geometry, MultiPoint): - # There are multiple candidate target points. Pick the one that appears last on the path. - # This helps us progress, and we don't have to worry about skipping significant parts of the path - # with such a small lookahead distance. - target_point = sorted( - intersection_geometry.geoms, key=lambda pt: refined_path_ls.project(pt) - )[-1] - target_point = Vector(*target_point.coords[0]) - else: - # We've gotten something strange. Fall back to a representative point as the target. - target_point = Vector(*intersection_geometry.representative_point().coords[0]) - - # Set appropriate heading and velocity, calculating actual speed we should aim - # for, so we don't overshoot if we cut a corner or are at the end of the path. - actual_speed = min(targetSpeed, (distance from self to target_point)/simulation().timestep) - heading = angle from self to target_point - take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) + # Follow the path until we replan, terminating early if we reach the end. + try: + do _WalkPathHelper(refined_path_ls, targetSpeed) for replanTime seconds + interrupt when distance from self to Vector(*refined_path_ls.coords[-1]) < terminationThresh: + abort take SetWalkingSpeedAction(0) From db51e2c1204aecccecd1ec7e9cd78d85d3a5b3e9 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 15:41:14 -0700 Subject: [PATCH 061/134] Further walk behavior fix. --- src/scenic/domains/driving/behaviors/walks.scenic | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 3ef47d9c8..b1629821b 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -8,7 +8,7 @@ from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): +def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1, network=None): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) @@ -88,7 +88,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1): start_path = shapely.force_2d(start_path) end_path = shapely.force_2d(end_path) - # Extract and reverse mid_path if needed + # Extract and reverse mid_path (if needed) mid_path = sorted(exterior_segments, key=lambda x: x.length)[0] mid_path_start = ShapelyPoint(mid_path.coords[0]) if (ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(start_path.coords[0])) @@ -105,13 +105,11 @@ behavior TieBreakingPause(): wait for Range(0.1, 0.5) seconds behavior _WalkPathHelper(path, targetSpeed): - # Start distAlong negative to ensure we can get back to the path if we somehow - # start away from it. - dist_along = -(distance from self to Vector(*path.coords[0])) + dist_along = 0 while True: # Determine target point, which will move along the path until we re-plan. - dist_along += targetSpeed - target_pt = Vector(*path.interpolate(max(0, dist_along)).coords[0]) + dist_along += targetSpeed*simulation().timestep + target_pt = Vector(*path.interpolate(dist_along).coords[0]) # Set appropriate heading and velocity, calculating actual speed we should aim # for, so we don't overshoot if we cut a corner or are at the end of the path. actual_speed = min(targetSpeed, (distance from self to target_pt)/simulation().timestep) @@ -145,7 +143,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 continue # Modify path to route around objects. - refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst) + refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst, network=_model.network) # If refined_path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. if refined_path_ls is None: From dca262945ba49dde4eda137f102ebabdef25ed9c Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 16:03:23 -0700 Subject: [PATCH 062/134] Fix grammar checksum --- src/scenic/syntax/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/scenic/syntax/__init__.py b/src/scenic/syntax/__init__.py index 8c9041b80..66d65ce1c 100644 --- a/src/scenic/syntax/__init__.py +++ b/src/scenic/syntax/__init__.py @@ -34,13 +34,13 @@ def buildParser(): ) with open(_checksumPath, "wb") as f: - f.write(getParserHash()) + f.write(getGrammarHash()) return result -def getParserHash(): - with open(_parserPath, "rb") as f: +def getGrammarHash(): + with open(_grammarPath, "rb") as f: data = f.read() return _hashlib.blake2b(data).digest() @@ -52,7 +52,7 @@ def checksumValid(): with open(_checksumPath, "rb") as f: checksum = f.read() - return checksum == getParserHash() + return checksum == getGrammarHash() if not _parserPath.exists() or not checksumValid(): From 59db4e25368b896b93daa954eddeee9706aca65a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 17:04:20 -0700 Subject: [PATCH 063/134] Further improvements to pedestrian walking. --- .../domains/driving/behaviors/walks.scenic | 56 +++++++++++++------ 1 file changed, 39 insertions(+), 17 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index b1629821b..543cdf577 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -1,3 +1,5 @@ +import collections +import math import shapely from shapely.geometry import GeometryCollection, MultiPolygon, Polygon, MultiLineString, LineString, MultiPoint, Point as ShapelyPoint @@ -8,26 +10,30 @@ from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1, network=None): +def getBugPath(actor, path_ls, backgroundObjects, obstPolyHist, bufferCalc): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) - # Compute the buffer amount based on our bounding radius and bufferConst - buffer_amount = shapely.minimum_bounding_radius(actor._boundingPolygon) + bufferConst + # Lambda to compute buffer const. + baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) # Compute the obstacle polygons - obstacle_multi_poly = shapely.union_all(list(obj._boundingPolygon.buffer(buffer_amount) for obj in backgroundObjects)) - if isinstance(obstacle_multi_poly, MultiPolygon): - obstacle_polys = obstacle_multi_poly.geoms + obstacle_polys = [obj._boundingPolygon.buffer(bufferCalc(obj))for obj in backgroundObjects] + obst_multi_poly = shapely.union_all(obstacle_polys) + hist_multi_poly = shapely.union_all(list(obstPolyHist) + [obst_multi_poly]) + if isinstance(hist_multi_poly, MultiPolygon): + obst_polys = hist_multi_poly.geoms assert all(isinstance(geom, Polygon) for geom in obstacle_polys) - elif isinstance(obstacle_multi_poly, Polygon): - obstacle_polys = [obstacle_multi_poly] + elif isinstance(hist_multi_poly, Polygon): + obst_polys = [hist_multi_poly] + elif hist_multi_poly.is_empty: + obst_polys = [] else: assert False # Refine path around obstacles, going from those with the largest boundary inwards # (to account for the rare case where an obstacle poly may be entirely contained in another) - for obstacle_poly in sorted(obstacle_polys, key=lambda x: x.boundary.length, reverse=True): + for obstacle_poly in sorted(obst_polys, key=lambda x: x.boundary.length, reverse=True): self_pt = ShapelyPoint(actor.position) target_pt = ShapelyPoint(path_ls.coords[-1]) if obstacle_poly.contains(target_pt): @@ -88,8 +94,16 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1, network=None): start_path = shapely.force_2d(start_path) end_path = shapely.force_2d(end_path) - # Extract and reverse mid_path (if needed) - mid_path = sorted(exterior_segments, key=lambda x: x.length)[0] + # Extract and reverse mid_path (if needed). If paths are very close in length, + # bias to the right. + if 0.95 < exterior_segments[0].length/exterior_segments[1].length < 1.05: + def angle_helper(ls): + actor.apparentHeadingTo(toVector(*ls.centroid)) + + exterior_segments.sort(key=lambda x: x.length) + else: + exterior_segments.sort(key=lambda x: x.length) + mid_path = exterior_segments[0] mid_path_start = ShapelyPoint(mid_path.coords[0]) if (ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(start_path.coords[0])) > ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(end_path.coords[0]))): @@ -98,7 +112,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferConst=1, network=None): path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - return path_ls + return path_ls, obst_multi_poly behavior TieBreakingPause(): take SetWalkingSpeedAction(0) @@ -116,11 +130,17 @@ behavior _WalkPathHelper(path, targetSpeed): heading = angle from self to target_pt take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) -behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.5, bufferConst=1): +behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, + terminationThresh=0.1, replanTime=0.5, obstHistory=2, + vehBuffer=1, nonVehBuffer=0.25): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") path_ls = path.lineString + obstacle_poly_hist = collections.deque(maxlen=math.ceil(obstHistory/replanTime)) + + bufferCalc = lambda obj: (shapely.minimum_bounding_radius(self._boundingPolygon) + + (vehBuffer if obj.isVehicle else nonVehBuffer)) while distance from self to path.end > terminationThresh: # Refine the path by dropping already traversed areas and adding a link to the start. @@ -133,17 +153,18 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0 # wait until it's clear. background_objects = [obj for obj in simulation().objects if obj is not self] immediate_path = shapely.ops.substring(refined_path_ls, 0, targetSpeed) - danger_objects = [obj for obj in background_objects if obj.speed > 0.1] - buffer_amount = shapely.minimum_bounding_radius(self._boundingPolygon) + bufferConst + danger_objects = [obj for obj in background_objects if obj.speed > 0.1 and obj.isVehicle] moving_obj_danger_zone = shapely.union_all( - list(obj._boundingPolygon.buffer(buffer_amount) for obj in danger_objects) + [obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in danger_objects] ) if immediate_path.intersects(moving_obj_danger_zone): do TieBreakingPause() continue # Modify path to route around objects. - refined_path_ls = getBugPath(self, refined_path_ls, background_objects, bufferConst=bufferConst, network=_model.network) + refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc) + obstacle_poly_hist.append(poly) + # If refined_path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. if refined_path_ls is None: @@ -173,6 +194,7 @@ behavior Walk(targetSpeed=None, backwards=None, avoidObstacles=True): network = _model.network + # TODO: Have pedestrians bias towards the appropriate side of the sidewalk based off road direction? while True: # If we're not currently in a walkable region, return to the closest one. if self.position not in network.walkableRegion: From c92a1e0748ff6a0a15a8b0fa687e24bfea4d9e7e Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 17:09:29 -0700 Subject: [PATCH 064/134] Increased pedestrian behavior replanning frequency. --- src/scenic/domains/driving/behaviors/walks.scenic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 543cdf577..91c5f45b3 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -131,7 +131,7 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=0.5, obstHistory=2, + terminationThresh=0.1, replanTime=0.1, obstHistory=2, vehBuffer=1, nonVehBuffer=0.25): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): From 418d6eeaa0f85d03b0888d424fa0f591973b0a1d Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 18:05:11 -0700 Subject: [PATCH 065/134] Further improvements to walking behavior. --- .../domains/driving/behaviors/walks.scenic | 17 +++++++++++------ src/scenic/simulators/metadrive/model.scenic | 2 +- src/scenic/simulators/metadrive/simulator.py | 9 +++++---- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 91c5f45b3..580f1d195 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -19,8 +19,12 @@ def getBugPath(actor, path_ls, backgroundObjects, obstPolyHist, bufferCalc): # Compute the obstacle polygons obstacle_polys = [obj._boundingPolygon.buffer(bufferCalc(obj))for obj in backgroundObjects] - obst_multi_poly = shapely.union_all(obstacle_polys) - hist_multi_poly = shapely.union_all(list(obstPolyHist) + [obst_multi_poly]) + fut_polys = [shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y)) + for t in [0.5, 1] for poly, obj in zip(obstacle_polys, backgroundObjects)] + + obst_multi_poly = shapely.union_all(obstacle_polys + fut_polys) + + hist_multi_poly = shapely.union_all(list(obstPolyHist) + fut_polys + [obst_multi_poly]) if isinstance(hist_multi_poly, MultiPolygon): obst_polys = hist_multi_poly.geoms assert all(isinstance(geom, Polygon) for geom in obstacle_polys) @@ -96,11 +100,12 @@ def getBugPath(actor, path_ls, backgroundObjects, obstPolyHist, bufferCalc): # Extract and reverse mid_path (if needed). If paths are very close in length, # bias to the right. - if 0.95 < exterior_segments[0].length/exterior_segments[1].length < 1.05: + # TODO: Bias to the appropriate driving direction + if 0.9 < exterior_segments[0].length/exterior_segments[1].length < 1.1: def angle_helper(ls): - actor.apparentHeadingTo(toVector(*ls.centroid)) + return actor.apparentHeadingTo(Vector(*ls.centroid.coords[0])) - exterior_segments.sort(key=lambda x: x.length) + exterior_segments.sort(key=lambda x: angle_helper(x)) else: exterior_segments.sort(key=lambda x: x.length) mid_path = exterior_segments[0] @@ -131,7 +136,7 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=0.1, obstHistory=2, + terminationThresh=0.1, replanTime=1, obstHistory=3, vehBuffer=1, nonVehBuffer=0.25): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): diff --git a/src/scenic/simulators/metadrive/model.scenic b/src/scenic/simulators/metadrive/model.scenic index 145795004..1d3308884 100644 --- a/src/scenic/simulators/metadrive/model.scenic +++ b/src/scenic/simulators/metadrive/model.scenic @@ -187,7 +187,7 @@ class Pedestrian(Pedestrian, MetaDriveActor, Walks): return True def setWalkingDirection(self, heading): - self._walking_direction = scenicToMetaDriveHeading(heading) + self._walking_direction = heading def setWalkingSpeed(self, speed): self._walking_speed = speed diff --git a/src/scenic/simulators/metadrive/simulator.py b/src/scenic/simulators/metadrive/simulator.py index 7410c7345..abba972ec 100644 --- a/src/scenic/simulators/metadrive/simulator.py +++ b/src/scenic/simulators/metadrive/simulator.py @@ -286,14 +286,15 @@ def executeActions(self, allActions): else: # For Pedestrians if obj._walking_direction is None: - obj._walking_direction = utils.scenicToMetaDriveHeading(obj.heading) + obj._walking_direction = obj.heading if obj._walking_speed is None: obj._walking_speed = obj.speed + metadrive_heading = utils.scenicToMetaDriveHeading(obj._walking_direction) direction = [ - math.cos(obj._walking_direction), - math.sin(obj._walking_direction), + math.cos(metadrive_heading), + math.sin(metadrive_heading), ] - obj.setOrientation(toOrientation(obj.heading)) + obj.setOrientation(toOrientation(obj._walking_direction)) obj.metaDriveActor.set_velocity(direction, obj._walking_speed) def step(self): From 73e5a90450bd12a8b1361823a143022f3e11ef57 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 13 Jul 2026 18:22:00 -0700 Subject: [PATCH 066/134] Further walking behavior improvements. --- .../domains/driving/behaviors/walks.scenic | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 580f1d195..dffc0f4df 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -10,27 +10,29 @@ from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, obstPolyHist, bufferCalc): +def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) # Lambda to compute buffer const. baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) - # Compute the obstacle polygons - obstacle_polys = [obj._boundingPolygon.buffer(bufferCalc(obj))for obj in backgroundObjects] - fut_polys = [shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y)) - for t in [0.5, 1] for poly, obj in zip(obstacle_polys, backgroundObjects)] - - obst_multi_poly = shapely.union_all(obstacle_polys + fut_polys) - - hist_multi_poly = shapely.union_all(list(obstPolyHist) + fut_polys + [obst_multi_poly]) - if isinstance(hist_multi_poly, MultiPolygon): - obst_polys = hist_multi_poly.geoms - assert all(isinstance(geom, Polygon) for geom in obstacle_polys) - elif isinstance(hist_multi_poly, Polygon): - obst_polys = [hist_multi_poly] - elif hist_multi_poly.is_empty: + # Compute the obstacle polygons, with some forward prediction. + obst_polys = {obj: obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in backgroundObjects} + obst_shifted_polys = {obj: shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y)) + for t in [0, 0.5, 1] for obj, poly in obst_polys.items()} + + # Only track non-vehicles as historicaly polys, as those are the only ones we will + # path around while moving. Vehicles are expected to yield to us. + hist_multi_poly = shapely.union_all([poly for obj, poly in obst_shifted_polys.items() if not obj.isVehicle]) + + obst_multi_poly = shapely.union_all(list(additionalPolys) + list(obst_shifted_polys.values())) + if isinstance(obst_multi_poly, MultiPolygon): + obst_polys = obst_multi_poly.geoms + assert all(isinstance(geom, Polygon) for geom in obst_polys) + elif isinstance(obst_multi_poly, Polygon): + obst_polys = [obst_multi_poly] + elif obst_multi_poly.is_empty: obst_polys = [] else: assert False @@ -44,7 +46,7 @@ def getBugPath(actor, path_ls, backgroundObjects, obstPolyHist, bufferCalc): # Check if target is inside the polygon. # If we're too close to the exterior point, return None. if obstacle_poly.distance(self_pt) < 0.01: - return None + return None, hist_multi_poly # Otherwise, truncate the path to the closest point on the exterior of the obstacle_poly. exterior_intersection = path_ls.intersection(obstacle_poly.exterior) @@ -117,7 +119,7 @@ def getBugPath(actor, path_ls, backgroundObjects, obstPolyHist, bufferCalc): path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - return path_ls, obst_multi_poly + return path_ls, hist_multi_poly behavior TieBreakingPause(): take SetWalkingSpeedAction(0) @@ -136,7 +138,7 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=1, obstHistory=3, + terminationThresh=0.1, replanTime=1, obstHistory=4, vehBuffer=1, nonVehBuffer=0.25): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): From 5dcb3d8855463ba8716309a498f69ea206945321 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 10:09:10 -0700 Subject: [PATCH 067/134] Debugging --- src/scenic/domains/driving/behaviors/walks.scenic | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index dffc0f4df..4534f8adb 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -8,6 +8,7 @@ from scenic.core.regions import toShapely from scenic.core.type_support import toVector from scenic.domains.driving.actions import * +DEBUG_POLY = None ## Pedestrian Behaviors def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): @@ -19,14 +20,18 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): # Compute the obstacle polygons, with some forward prediction. obst_polys = {obj: obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in backgroundObjects} - obst_shifted_polys = {obj: shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y)) - for t in [0, 0.5, 1] for obj, poly in obst_polys.items()} + obst_polys.update({obj: shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y)) + for t in [0.5, 1] for obj, poly in obst_polys.items() if not obj.isVehicle}) # Only track non-vehicles as historicaly polys, as those are the only ones we will # path around while moving. Vehicles are expected to yield to us. - hist_multi_poly = shapely.union_all([poly for obj, poly in obst_shifted_polys.items() if not obj.isVehicle]) + hist_multi_poly = shapely.union_all([poly for obj, poly in obst_polys.items() if not obj.isVehicle]) + + obst_multi_poly = shapely.union_all(list(additionalPolys) + list(obst_polys.values())) + + global DEBUG_POLY + DEBUG_POLY = obst_multi_poly - obst_multi_poly = shapely.union_all(list(additionalPolys) + list(obst_shifted_polys.values())) if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms assert all(isinstance(geom, Polygon) for geom in obst_polys) From 353781b2d48e6a60b2589d5db6d27d505ec3f52e Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 10:12:39 -0700 Subject: [PATCH 068/134] More debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 3 +-- src/scenic/domains/driving/simulators.py | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 4534f8adb..30aaba84e 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -8,8 +8,6 @@ from scenic.core.regions import toShapely from scenic.core.type_support import toVector from scenic.domains.driving.actions import * -DEBUG_POLY = None - ## Pedestrian Behaviors def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): """ Refine a walking path using a Bug algorithm approach.""" @@ -29,6 +27,7 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): obst_multi_poly = shapely.union_all(list(additionalPolys) + list(obst_polys.values())) + from scenic.domains.driving.simulators import DEBUG_POLY global DEBUG_POLY DEBUG_POLY = obst_multi_poly diff --git a/src/scenic/domains/driving/simulators.py b/src/scenic/domains/driving/simulators.py index 1fa1c1b64..9cbde584d 100644 --- a/src/scenic/domains/driving/simulators.py +++ b/src/scenic/domains/driving/simulators.py @@ -6,6 +6,8 @@ PIDLongitudinalController, ) +DEBUG_POLY = None + class DrivingSimulator(Simulator): """A `Simulator` supporting the driving domain.""" From 7bf53b89366e4ed2fa750c33c0aecf04289f43dc Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 10:21:42 -0700 Subject: [PATCH 069/134] Debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 30aaba84e..84eee69bb 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -30,6 +30,7 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): from scenic.domains.driving.simulators import DEBUG_POLY global DEBUG_POLY DEBUG_POLY = obst_multi_poly + print(f"BACKGROUND OBJS: {backgroundObjects}") if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms From 6fa347a94d7ff9667ba350b3438d435b3558fd5f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 10:22:51 -0700 Subject: [PATCH 070/134] More debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 84eee69bb..21cac0ebe 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -30,7 +30,8 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): from scenic.domains.driving.simulators import DEBUG_POLY global DEBUG_POLY DEBUG_POLY = obst_multi_poly - print(f"BACKGROUND OBJS: {backgroundObjects}") + print(f"BACKGROUND OBJS: {len(backgroundObjects)}") + print(f"OBST POLY: {obst_multi_poly}") if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms From c8260059afe2a7bf1c9c57cc4cc42ef7a7898e50 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 10:55:36 -0700 Subject: [PATCH 071/134] Added erosion factor to walk behavior to smooth return to path. --- .../domains/driving/behaviors/walks.scenic | 40 +++++++++++-------- src/scenic/domains/driving/simulators.py | 2 - src/scenic/simulators/newtonian/simulator.py | 1 + 3 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 21cac0ebe..1892ca9ac 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -17,21 +17,15 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) # Compute the obstacle polygons, with some forward prediction. - obst_polys = {obj: obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in backgroundObjects} - obst_polys.update({obj: shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y)) - for t in [0.5, 1] for obj, poly in obst_polys.items() if not obj.isVehicle}) + obst_polys = [(obj, obj._boundingPolygon.buffer(bufferCalc(obj))) for obj in backgroundObjects] + obst_polys += [(obj, shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y))) + for t in [0.5, 1] for obj, poly in obst_polys if not obj.isVehicle] # Only track non-vehicles as historicaly polys, as those are the only ones we will # path around while moving. Vehicles are expected to yield to us. - hist_multi_poly = shapely.union_all([poly for obj, poly in obst_polys.items() if not obj.isVehicle]) + hist_multi_poly = shapely.union_all([poly for obj, poly in obst_polys if not obj.isVehicle]) - obst_multi_poly = shapely.union_all(list(additionalPolys) + list(obst_polys.values())) - - from scenic.domains.driving.simulators import DEBUG_POLY - global DEBUG_POLY - DEBUG_POLY = obst_multi_poly - print(f"BACKGROUND OBJS: {len(backgroundObjects)}") - print(f"OBST POLY: {obst_multi_poly}") + obst_multi_poly = shapely.union_all([p for _,p in obst_polys] + list(additionalPolys)) if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms @@ -112,7 +106,6 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): if 0.9 < exterior_segments[0].length/exterior_segments[1].length < 1.1: def angle_helper(ls): return actor.apparentHeadingTo(Vector(*ls.centroid.coords[0])) - exterior_segments.sort(key=lambda x: angle_helper(x)) else: exterior_segments.sort(key=lambda x: x.length) @@ -125,6 +118,17 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) + import matplotlib.pyplot as plt + from scenic.core.geometry import plotPolygon + from scenic.syntax.veneer import simulation + if actor.name == "pedA" and simulation().currentRealTime == int(simulation().currentRealTime) and simulation().currentRealTime > 0: + simulation().scene.workspace.network.show() + for obj in simulation().objects: + obj.show2D(simulation().scene.workspace, plt) + simulation().scene.workspace.zoomAround(plt, simulation().objects) + plotPolygon(obst_multi_poly, plt, style="c--") + plt.show() + return path_ls, hist_multi_poly behavior TieBreakingPause(): @@ -144,13 +148,13 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=1, obstHistory=4, - vehBuffer=1, nonVehBuffer=0.25): + terminationThresh=0.1, replanTime=0.5, obstHistory=4, + vehBuffer=1, nonVehBuffer=0.25, erosionFactor=1): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") path_ls = path.lineString - obstacle_poly_hist = collections.deque(maxlen=math.ceil(obstHistory/replanTime)) + obstacle_poly_hist = [] bufferCalc = lambda obj: (shapely.minimum_bounding_radius(self._boundingPolygon) + (vehBuffer if obj.isVehicle else nonVehBuffer)) @@ -176,7 +180,11 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, # Modify path to route around objects. refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc) - obstacle_poly_hist.append(poly) + + # Update and slightly erode the obstacle poly history + stepErosionFactor = min(replanTime/obstHistory, 1) * erosionFactor + obstacle_poly_hist = [p.buffer(-stepErosionFactor) for p in obstacle_poly_hist] + [poly] + obstacle_poly_hist = obstacle_poly_hist[-math.ceil(obstHistory/replanTime):] # If refined_path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. diff --git a/src/scenic/domains/driving/simulators.py b/src/scenic/domains/driving/simulators.py index 9cbde584d..1fa1c1b64 100644 --- a/src/scenic/domains/driving/simulators.py +++ b/src/scenic/domains/driving/simulators.py @@ -6,8 +6,6 @@ PIDLongitudinalController, ) -DEBUG_POLY = None - class DrivingSimulator(Simulator): """A `Simulator` supporting the driving domain.""" diff --git a/src/scenic/simulators/newtonian/simulator.py b/src/scenic/simulators/newtonian/simulator.py index b9945e145..61876e99b 100644 --- a/src/scenic/simulators/newtonian/simulator.py +++ b/src/scenic/simulators/newtonian/simulator.py @@ -214,6 +214,7 @@ def step(self): if obj.control["speed"] is not None else obj.speed ) + obj.heading = h obj.velocity = Vector(0, s).rotatedBy(h) obj.speed = obj.velocity.norm() # 2) Vehicle: throttle/brake/steer physics From 1ae16dff7af8ffdbed7ed58da581c8a2f296138a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 10:59:47 -0700 Subject: [PATCH 072/134] Fixed aspect ratio on debug plot. --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 1892ca9ac..72c1fb679 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -119,6 +119,7 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): path_ls = shapely.remove_repeated_points(path_ls) import matplotlib.pyplot as plt + plt.gca().set_aspect("equal") from scenic.core.geometry import plotPolygon from scenic.syntax.veneer import simulation if actor.name == "pedA" and simulation().currentRealTime == int(simulation().currentRealTime) and simulation().currentRealTime > 0: From 5888ac0eb5668ad72b709791bbfa464d333e4b58 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 11:13:41 -0700 Subject: [PATCH 073/134] Added debug prints. --- src/scenic/domains/driving/behaviors/walks.scenic | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 72c1fb679..2049dbd4d 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -176,6 +176,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, [obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in danger_objects] ) if immediate_path.intersects(moving_obj_danger_zone): + print("PAUSE 1") do TieBreakingPause() continue @@ -190,6 +191,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, # If refined_path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. if refined_path_ls is None: + print("PAUSE 2") do TieBreakingPause() continue @@ -197,6 +199,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, try: do _WalkPathHelper(refined_path_ls, targetSpeed) for replanTime seconds interrupt when distance from self to Vector(*refined_path_ls.coords[-1]) < terminationThresh: + print("INTERRUPT") abort take SetWalkingSpeedAction(0) From ed4545af90193b03301570f2e6fb20ab7de8d7e5 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 11:15:34 -0700 Subject: [PATCH 074/134] Remove debug visualization. --- .../domains/driving/behaviors/walks.scenic | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 2049dbd4d..dcb0a1c91 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -118,17 +118,17 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - import matplotlib.pyplot as plt - plt.gca().set_aspect("equal") - from scenic.core.geometry import plotPolygon - from scenic.syntax.veneer import simulation - if actor.name == "pedA" and simulation().currentRealTime == int(simulation().currentRealTime) and simulation().currentRealTime > 0: - simulation().scene.workspace.network.show() - for obj in simulation().objects: - obj.show2D(simulation().scene.workspace, plt) - simulation().scene.workspace.zoomAround(plt, simulation().objects) - plotPolygon(obst_multi_poly, plt, style="c--") - plt.show() + # import matplotlib.pyplot as plt + # plt.gca().set_aspect("equal") + # from scenic.core.geometry import plotPolygon + # from scenic.syntax.veneer import simulation + # if actor.name == "pedA" and simulation().currentRealTime == int(simulation().currentRealTime) and simulation().currentRealTime > 0: + # simulation().scene.workspace.network.show() + # for obj in simulation().objects: + # obj.show2D(simulation().scene.workspace, plt) + # simulation().scene.workspace.zoomAround(plt, simulation().objects) + # plotPolygon(obst_multi_poly, plt, style="c--") + # plt.show() return path_ls, hist_multi_poly From 1563c4c2a47bbde1faf1b0aa9dcee28ab70378b2 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 14:55:29 -0700 Subject: [PATCH 075/134] Walking tweaks. --- src/scenic/domains/driving/behaviors/walks.scenic | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index dcb0a1c91..990633f12 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -149,8 +149,8 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=0.5, obstHistory=4, - vehBuffer=1, nonVehBuffer=0.25, erosionFactor=1): + terminationThresh=0.1, replanTime=1, obstHistory=6, + vehBuffer=1, nonVehBuffer=0.5, erosionFactor=0.75): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") From 69142e62908cc052574e394888d911d6fee6db15 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:17:12 -0700 Subject: [PATCH 076/134] Lookahead time now scales with replan time. --- src/scenic/domains/driving/behaviors/walks.scenic | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 990633f12..d87e5049b 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -9,7 +9,7 @@ from scenic.core.type_support import toVector from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): +def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc, lookaheadTime): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) @@ -18,8 +18,9 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc): # Compute the obstacle polygons, with some forward prediction. obst_polys = [(obj, obj._boundingPolygon.buffer(bufferCalc(obj))) for obj in backgroundObjects] + lookahead_vals = [0.5*t for t in range(1,2*lookaheadTime+1)] obst_polys += [(obj, shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y))) - for t in [0.5, 1] for obj, poly in obst_polys if not obj.isVehicle] + for t in lookahead_vals for obj, poly in obst_polys if not obj.isVehicle] # Only track non-vehicles as historicaly polys, as those are the only ones we will # path around while moving. Vehicles are expected to yield to us. @@ -150,7 +151,7 @@ behavior _WalkPathHelper(path, targetSpeed): behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=1, obstHistory=6, - vehBuffer=1, nonVehBuffer=0.5, erosionFactor=0.75): + vehBuffer=1, nonVehBuffer=0.25, erosionFactor=0.75): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") @@ -181,7 +182,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, continue # Modify path to route around objects. - refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc) + refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc, replanTime) # Update and slightly erode the obstacle poly history stepErosionFactor = min(replanTime/obstHistory, 1) * erosionFactor From 187ecf7114a8737092f6c51b91e60acd7afd9caf Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:22:02 -0700 Subject: [PATCH 077/134] Split out lookahead time into its own param. --- src/scenic/domains/driving/behaviors/walks.scenic | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index d87e5049b..8f61d56ad 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -150,7 +150,7 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=1, obstHistory=6, + terminationThresh=0.1, replanTime=1, obstHistory=6, lookaheadTime=2 vehBuffer=1, nonVehBuffer=0.25, erosionFactor=0.75): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): @@ -182,7 +182,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, continue # Modify path to route around objects. - refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc, replanTime) + refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc, lookaheadTime) # Update and slightly erode the obstacle poly history stepErosionFactor = min(replanTime/obstHistory, 1) * erosionFactor From 419b685410c87255aa71ce7c8f3d581661d4cdc9 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:22:36 -0700 Subject: [PATCH 078/134] Fix typo. --- src/scenic/domains/driving/behaviors/walks.scenic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 8f61d56ad..34882acc8 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -150,7 +150,7 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=1, obstHistory=6, lookaheadTime=2 + terminationThresh=0.1, replanTime=1, obstHistory=6, lookaheadTime=2, vehBuffer=1, nonVehBuffer=0.25, erosionFactor=0.75): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): From fa33b13785bbd8b49d29a390204296a4cd98d542 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:38:21 -0700 Subject: [PATCH 079/134] Debug --- src/scenic/domains/driving/behaviors/walks.scenic | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 34882acc8..ec27dbc9a 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -146,11 +146,12 @@ behavior _WalkPathHelper(path, targetSpeed): # Set appropriate heading and velocity, calculating actual speed we should aim # for, so we don't overshoot if we cut a corner or are at the end of the path. actual_speed = min(targetSpeed, (distance from self to target_pt)/simulation().timestep) + print(f"EXPECTED POINT: {target_point}") heading = angle from self to target_pt take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=1, obstHistory=6, lookaheadTime=2, + terminationThresh=0.1, replanTime=0.5, obstHistory=6, lookaheadTime=2, vehBuffer=1, nonVehBuffer=0.25, erosionFactor=0.75): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): From 3502d3c992d5f6934036d0dbb82a4a36aadc84b3 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:39:46 -0700 Subject: [PATCH 080/134] Fix typo. --- src/scenic/domains/driving/behaviors/walks.scenic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index ec27dbc9a..caa0d96eb 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -146,7 +146,7 @@ behavior _WalkPathHelper(path, targetSpeed): # Set appropriate heading and velocity, calculating actual speed we should aim # for, so we don't overshoot if we cut a corner or are at the end of the path. actual_speed = min(targetSpeed, (distance from self to target_pt)/simulation().timestep) - print(f"EXPECTED POINT: {target_point}") + print(f"EXPECTED POINT: {target_pt}") heading = angle from self to target_pt take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) From 0963b5f0cd7e2bd9b734d51f26d446e8633b8614 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:58:56 -0700 Subject: [PATCH 081/134] Debug --- src/scenic/domains/driving/actions.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index 6d61c27db..ca255a03d 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -56,9 +56,11 @@ class Walks: """ def setWalkingDirection(self, heading): + print("SETTING DIRECTION") self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): + print("SETTING SPEED") self.setVelocity(Vector(0, speed).rotatedBy(self.heading)) From 42d399840dae979bbdd4d3f1f9ecb0059b4863d3 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 15:59:40 -0700 Subject: [PATCH 082/134] More debug. --- src/scenic/domains/driving/actions.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index ca255a03d..e44b1a256 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -56,11 +56,11 @@ class Walks: """ def setWalkingDirection(self, heading): - print("SETTING DIRECTION") + print(f"SETTING HEADING: {heading}") self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): - print("SETTING SPEED") + print(f"SETTING SPEED: {speed}") self.setVelocity(Vector(0, speed).rotatedBy(self.heading)) From 429385b70122e2af786b0a47611b42e02be5e247 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 16:03:16 -0700 Subject: [PATCH 083/134] More debug --- src/scenic/domains/driving/actions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index e44b1a256..9f5c84a83 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -60,7 +60,7 @@ def setWalkingDirection(self, heading): self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): - print(f"SETTING SPEED: {speed}") + print(f"SETTING SPEED: {speed}. HEADING IS {self.heading}") self.setVelocity(Vector(0, speed).rotatedBy(self.heading)) From 60065d3adb76b7f6be65d7bb811687f9d4547e05 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 14 Jul 2026 16:09:35 -0700 Subject: [PATCH 084/134] Remove debug. --- src/scenic/domains/driving/actions.py | 2 -- src/scenic/domains/driving/behaviors/walks.scenic | 1 - 2 files changed, 3 deletions(-) diff --git a/src/scenic/domains/driving/actions.py b/src/scenic/domains/driving/actions.py index 9f5c84a83..6d61c27db 100644 --- a/src/scenic/domains/driving/actions.py +++ b/src/scenic/domains/driving/actions.py @@ -56,11 +56,9 @@ class Walks: """ def setWalkingDirection(self, heading): - print(f"SETTING HEADING: {heading}") self.setOrientation(toOrientation(heading)) def setWalkingSpeed(self, speed): - print(f"SETTING SPEED: {speed}. HEADING IS {self.heading}") self.setVelocity(Vector(0, speed).rotatedBy(self.heading)) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index caa0d96eb..5b2977c28 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -146,7 +146,6 @@ behavior _WalkPathHelper(path, targetSpeed): # Set appropriate heading and velocity, calculating actual speed we should aim # for, so we don't overshoot if we cut a corner or are at the end of the path. actual_speed = min(targetSpeed, (distance from self to target_pt)/simulation().timestep) - print(f"EXPECTED POINT: {target_pt}") heading = angle from self to target_pt take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) From bb59d3dde9eaf30bdaa8e5b077e7f5bc9d39773d Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 13:39:05 -0700 Subject: [PATCH 085/134] Much improved walking behavior. --- .../domains/driving/behaviors/walks.scenic | 115 +++++++++++------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 5b2977c28..f561b76d8 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -9,24 +9,27 @@ from scenic.core.type_support import toVector from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc, lookaheadTime): +def getBugPath(actor, path_ls, backgroundObjects, additionalPoly, bufferCalc, erosionFactor, lookaheadTime): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) + orig_path_ls = path_ls #TODO: TEMP # Lambda to compute buffer const. baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) - # Compute the obstacle polygons, with some forward prediction. + # Compute the obstacle polygons, accounting for the plan of objects that have already logged it. obst_polys = [(obj, obj._boundingPolygon.buffer(bufferCalc(obj))) for obj in backgroundObjects] - lookahead_vals = [0.5*t for t in range(1,2*lookaheadTime+1)] - obst_polys += [(obj, shapely.transform(poly, lambda x: x + (t*obj.velocity.x, t*obj.velocity.y))) - for t in lookahead_vals for obj, poly in obst_polys if not obj.isVehicle] + def future_poly_helper(obj): + planned_path, planned_speed = obj._planData + trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) + return trimmed_path.buffer(bufferCalc(obj) + shapely.minimum_bounding_radius(obj._boundingPolygon)) + future_polys = [future_poly_helper(obj) for obj in backgroundObjects + if not obj.isVehicle and getattr(obj, "_planData", None) is not None] # Only track non-vehicles as historicaly polys, as those are the only ones we will # path around while moving. Vehicles are expected to yield to us. hist_multi_poly = shapely.union_all([poly for obj, poly in obst_polys if not obj.isVehicle]) - - obst_multi_poly = shapely.union_all([p for _,p in obst_polys] + list(additionalPolys)) + obst_multi_poly = shapely.union_all([p for _,p in obst_polys] + future_polys + [additionalPoly]) if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms @@ -41,8 +44,10 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc, l # Refine path around obstacles, going from those with the largest boundary inwards # (to account for the rare case where an obstacle poly may be entirely contained in another) for obstacle_poly in sorted(obst_polys, key=lambda x: x.boundary.length, reverse=True): - self_pt = ShapelyPoint(actor.position) - target_pt = ShapelyPoint(path_ls.coords[-1]) + self_pt = shapely.force_2d(ShapelyPoint(actor.position)) + target_pt = shapely.force_2d(ShapelyPoint(path_ls.coords[-1])) + + # TODO: Better handling so the pedestrian keeps walking towards goal inside a large poly. if obstacle_poly.contains(target_pt): # Check if target is inside the polygon. # If we're too close to the exterior point, return None. @@ -66,16 +71,24 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc, l intersection_points.append(path_ls.interpolate(path_ls.project(self_pt))) if isinstance(exterior_intersection, ShapelyPoint): - assert obstacle_poly.contains(ShapelyPoint(path_ls.coords[0])) - intersection_points.append(exterior_intersection) + # If we are in the obstacle poly, add the intersection point to help guide us out. + # Otherwise, our destination is on the border of the poly itself, and we simply should continue. + if obstacle_poly.contains(self_pt): + + intersection_points.append(exterior_intersection) + else: + continue elif isinstance(exterior_intersection, LineString): - instersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom,coords[1])] + intersection_points += [ + ShapelyPoint(exterior_intersection.coords[0]), + ShapelyPoint(exterior_intersection.coords[1]) + ] elif isinstance(exterior_intersection, (MultiPoint, MultiLineString, GeometryCollection)): for geom in exterior_intersection.geoms: if isinstance(geom, ShapelyPoint): intersection_points.append(geom) elif isinstance(geom, LineString): - instersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom,coords[1])] + intersection_points += [ShapelyPoint(geom.coords[0]), ShapelyPoint(geom.coords[1])] else: assert False @@ -101,41 +114,53 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPolys, bufferCalc, l start_path = shapely.force_2d(start_path) end_path = shapely.force_2d(end_path) - # Extract and reverse mid_path (if needed). If paths are very close in length, + # Extract the mid_path. If paths are very close in length, # bias to the right. # TODO: Bias to the appropriate driving direction - if 0.9 < exterior_segments[0].length/exterior_segments[1].length < 1.1: + if 0.7 < exterior_segments[0].length/exterior_segments[1].length < 1.3: def angle_helper(ls): return actor.apparentHeadingTo(Vector(*ls.centroid.coords[0])) exterior_segments.sort(key=lambda x: angle_helper(x)) else: exterior_segments.sort(key=lambda x: x.length) mid_path = exterior_segments[0] - mid_path_start = ShapelyPoint(mid_path.coords[0]) + mid_path = shapely.force_2d(mid_path) + + # Reverse the mid path if needed. if (ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(start_path.coords[0])) > ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(end_path.coords[0]))): mid_path = mid_path.reverse() + # If the closest point on the mid path is very close, cut start path short + # and aim directly for it. This helps avoid backtracking loop. + if self_pt.distance(mid_path) < 1: + mid_path = shapely.ops.substring(mid_path, mid_path.project(self_pt, normalized=True), 1, normalized=True) + start_pt = ShapelyPoint(mid_path.coords[0]) + start_path = LineString([self_pt, start_pt]) + path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - # import matplotlib.pyplot as plt - # plt.gca().set_aspect("equal") - # from scenic.core.geometry import plotPolygon # from scenic.syntax.veneer import simulation - # if actor.name == "pedA" and simulation().currentRealTime == int(simulation().currentRealTime) and simulation().currentRealTime > 0: + # if simulation().currentRealTime > 4: + # import matplotlib.pyplot as plt + # plt.gca().set_aspect("equal") + # from scenic.core.geometry import plotPolygon + # from scenic.syntax.veneer import simulation # simulation().scene.workspace.network.show() # for obj in simulation().objects: # obj.show2D(simulation().scene.workspace, plt) # simulation().scene.workspace.zoomAround(plt, simulation().objects) # plotPolygon(obst_multi_poly, plt, style="c--") + # plotPolygon(orig_path_ls, plt, style="y-") + # plotPolygon(path_ls, plt, style="g--") # plt.show() return path_ls, hist_multi_poly behavior TieBreakingPause(): take SetWalkingSpeedAction(0) - wait for Range(0.1, 0.5) seconds + wait for Range(0, 0.5) seconds behavior _WalkPathHelper(path, targetSpeed): dist_along = 0 @@ -149,60 +174,65 @@ behavior _WalkPathHelper(path, targetSpeed): heading = angle from self to target_pt take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) +import matplotlib.pyplot as plt +from scenic.core.geometry import plotPolygon +from scenic.syntax.veneer import simulation + behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=0.5, obstHistory=6, lookaheadTime=2, - vehBuffer=1, nonVehBuffer=0.25, erosionFactor=0.75): + terminationThresh=0.1, replanTime=1, lookaheadTime=4, + vehBuffer=1, nonVehBuffer=0.2, erosionFactor=0.3): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") - path_ls = path.lineString - obstacle_poly_hist = [] + obstacle_poly = shapely.union_all([]) bufferCalc = lambda obj: (shapely.minimum_bounding_radius(self._boundingPolygon) + (vehBuffer if obj.isVehicle else nonVehBuffer)) + while distance from self to path.end > terminationThresh: + path_ls = shapely.force_2d(path.lineString) + self_pt = (self.position.x, self.position.y) + # Refine the path by dropping already traversed areas and adding a link to the start. - start_s = path_ls.project(ShapelyPoint(self.position), normalized=True) - refined_path_ls = shapely.ops.substring(path_ls, start_s, 1, normalized=True) - refined_path_ls = LineString([self.position] + list(refined_path_ls.coords)) - refined_path_ls = shapely.remove_repeated_points(refined_path_ls) + start_s = path_ls.project(ShapelyPoint(self_pt), normalized=True) + path_ls = shapely.ops.substring(path_ls, start_s, 1, normalized=True) + path_ls = LineString([self_pt] + list(path_ls.coords)) + path_ls = shapely.remove_repeated_points(path_ls) # If our immediate path has us cross through any objects in motion, stop and # wait until it's clear. background_objects = [obj for obj in simulation().objects if obj is not self] - immediate_path = shapely.ops.substring(refined_path_ls, 0, targetSpeed) + immediate_path = shapely.ops.substring(path_ls, 0, targetSpeed) danger_objects = [obj for obj in background_objects if obj.speed > 0.1 and obj.isVehicle] moving_obj_danger_zone = shapely.union_all( [obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in danger_objects] ) if immediate_path.intersects(moving_obj_danger_zone): - print("PAUSE 1") do TieBreakingPause() continue # Modify path to route around objects. - refined_path_ls, poly = getBugPath(self, refined_path_ls, background_objects, obstacle_poly_hist, bufferCalc, lookaheadTime) - + path_ls, poly = getBugPath(self, path_ls, background_objects, obstacle_poly, bufferCalc, erosionFactor, lookaheadTime) + self._planData = path_ls, targetSpeed + # Update and slightly erode the obstacle poly history - stepErosionFactor = min(replanTime/obstHistory, 1) * erosionFactor - obstacle_poly_hist = [p.buffer(-stepErosionFactor) for p in obstacle_poly_hist] + [poly] - obstacle_poly_hist = obstacle_poly_hist[-math.ceil(obstHistory/replanTime):] + # obstacle_poly = obstacle_poly.union(poly).buffer(3*erosionFactor).buffer(-4*erosionFactor) - # If refined_path_ls is None, our goal is inside the danger zone and we can't + # If path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. - if refined_path_ls is None: - print("PAUSE 2") + if path_ls is None: do TieBreakingPause() continue # Follow the path until we replan, terminating early if we reach the end. try: - do _WalkPathHelper(refined_path_ls, targetSpeed) for replanTime seconds - interrupt when distance from self to Vector(*refined_path_ls.coords[-1]) < terminationThresh: - print("INTERRUPT") + do _WalkPathHelper(path_ls, targetSpeed) for replanTime seconds + interrupt when distance from self to Vector(*path_ls.coords[-1]) < terminationThresh: abort + self._planData = None + take SetWalkingSpeedAction(0) behavior WalkTo(target, targetSpeed, *, avoidObstacles=True): @@ -250,6 +280,7 @@ behavior Walk(targetSpeed=None, backwards=None, avoidObstacles=True): continue # We have no valid next moves. Terminate the behavior. + # TODO: Turn around instead? return # TODO: This uses WalkTowardsAction which doesn't exist. From 102baa26ac076e6c3fd145c299b6b32caa3bbaed Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 13:45:35 -0700 Subject: [PATCH 086/134] Cleanup --- src/scenic/domains/driving/behaviors/walks.scenic | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index f561b76d8..efee1cce2 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -9,7 +9,7 @@ from scenic.core.type_support import toVector from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, additionalPoly, bufferCalc, erosionFactor, lookaheadTime): +def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) orig_path_ls = path_ls #TODO: TEMP @@ -29,7 +29,7 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPoly, bufferCalc, er # Only track non-vehicles as historicaly polys, as those are the only ones we will # path around while moving. Vehicles are expected to yield to us. hist_multi_poly = shapely.union_all([poly for obj, poly in obst_polys if not obj.isVehicle]) - obst_multi_poly = shapely.union_all([p for _,p in obst_polys] + future_polys + [additionalPoly]) + obst_multi_poly = shapely.union_all([p for _,p in obst_polys] + future_polys) if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms @@ -52,7 +52,7 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPoly, bufferCalc, er # Check if target is inside the polygon. # If we're too close to the exterior point, return None. if obstacle_poly.distance(self_pt) < 0.01: - return None, hist_multi_poly + return None # Otherwise, truncate the path to the closest point on the exterior of the obstacle_poly. exterior_intersection = path_ls.intersection(obstacle_poly.exterior) @@ -156,7 +156,7 @@ def getBugPath(actor, path_ls, backgroundObjects, additionalPoly, bufferCalc, er # plotPolygon(path_ls, plt, style="g--") # plt.show() - return path_ls, hist_multi_poly + return path_ls behavior TieBreakingPause(): take SetWalkingSpeedAction(0) @@ -184,12 +184,10 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") - obstacle_poly = shapely.union_all([]) bufferCalc = lambda obj: (shapely.minimum_bounding_radius(self._boundingPolygon) + (vehBuffer if obj.isVehicle else nonVehBuffer)) - while distance from self to path.end > terminationThresh: path_ls = shapely.force_2d(path.lineString) self_pt = (self.position.x, self.position.y) @@ -213,12 +211,9 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, continue # Modify path to route around objects. - path_ls, poly = getBugPath(self, path_ls, background_objects, obstacle_poly, bufferCalc, erosionFactor, lookaheadTime) + path_ls = getBugPath(self, path_ls, background_objects, bufferCalc, lookaheadTime) self._planData = path_ls, targetSpeed - # Update and slightly erode the obstacle poly history - # obstacle_poly = obstacle_poly.union(poly).buffer(3*erosionFactor).buffer(-4*erosionFactor) - # If path_ls is None, our goal is inside the danger zone and we can't # proceed further right now. if path_ls is None: From 8cd98d1355e1895d871704b7309373b1236d2739 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 14:33:54 -0700 Subject: [PATCH 087/134] Default modifications. --- .../domains/driving/behaviors/walks.scenic | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index efee1cce2..cdb39f2b5 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -141,20 +141,20 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime): path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - # from scenic.syntax.veneer import simulation - # if simulation().currentRealTime > 4: - # import matplotlib.pyplot as plt - # plt.gca().set_aspect("equal") - # from scenic.core.geometry import plotPolygon - # from scenic.syntax.veneer import simulation - # simulation().scene.workspace.network.show() - # for obj in simulation().objects: - # obj.show2D(simulation().scene.workspace, plt) - # simulation().scene.workspace.zoomAround(plt, simulation().objects) - # plotPolygon(obst_multi_poly, plt, style="c--") - # plotPolygon(orig_path_ls, plt, style="y-") - # plotPolygon(path_ls, plt, style="g--") - # plt.show() + from scenic.syntax.veneer import simulation + if simulation().currentRealTime > 4: + import matplotlib.pyplot as plt + plt.gca().set_aspect("equal") + from scenic.core.geometry import plotPolygon + from scenic.syntax.veneer import simulation + simulation().scene.workspace.network.show() + for obj in simulation().objects: + obj.show2D(simulation().scene.workspace, plt) + simulation().scene.workspace.zoomAround(plt, simulation().objects) + plotPolygon(obst_multi_poly, plt, style="c--") + plotPolygon(orig_path_ls, plt, style="y-") + plotPolygon(path_ls, plt, style="g--") + plt.show() return path_ls @@ -179,7 +179,7 @@ from scenic.core.geometry import plotPolygon from scenic.syntax.veneer import simulation behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=1, lookaheadTime=4, + terminationThresh=0.1, replanTime=0.5, lookaheadTime=4, vehBuffer=1, nonVehBuffer=0.2, erosionFactor=0.3): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): @@ -202,7 +202,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, # wait until it's clear. background_objects = [obj for obj in simulation().objects if obj is not self] immediate_path = shapely.ops.substring(path_ls, 0, targetSpeed) - danger_objects = [obj for obj in background_objects if obj.speed > 0.1 and obj.isVehicle] + danger_objects = [obj for obj in background_objects if obj.speed > 0.44 and obj.isVehicle] moving_obj_danger_zone = shapely.union_all( [obj._boundingPolygon.buffer(bufferCalc(obj)) for obj in danger_objects] ) From 1fd127db53cf7d1c2a6b8e11188e9093de07874c Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 14:46:17 -0700 Subject: [PATCH 088/134] Weird bug with buffer size. --- src/scenic/domains/driving/behaviors/walks.scenic | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index cdb39f2b5..15c42e816 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -9,7 +9,7 @@ from scenic.core.type_support import toVector from scenic.domains.driving.actions import * ## Pedestrian Behaviors -def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime): +def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, vehBuffer, nonVehBuffer): """ Refine a walking path using a Bug algorithm approach.""" assert isinstance(path_ls, LineString) orig_path_ls = path_ls #TODO: TEMP @@ -18,7 +18,8 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime): baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) # Compute the obstacle polygons, accounting for the plan of objects that have already logged it. - obst_polys = [(obj, obj._boundingPolygon.buffer(bufferCalc(obj))) for obj in backgroundObjects] + raw_obst_polys = [obj._boundingPolygon.buffer(baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer)) + for obj in backgroundObjects] def future_poly_helper(obj): planned_path, planned_speed = obj._planData trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) @@ -26,10 +27,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime): future_polys = [future_poly_helper(obj) for obj in backgroundObjects if not obj.isVehicle and getattr(obj, "_planData", None) is not None] - # Only track non-vehicles as historicaly polys, as those are the only ones we will - # path around while moving. Vehicles are expected to yield to us. - hist_multi_poly = shapely.union_all([poly for obj, poly in obst_polys if not obj.isVehicle]) - obst_multi_poly = shapely.union_all([p for _,p in obst_polys] + future_polys) + obst_multi_poly = shapely.union_all(raw_obst_polys + future_polys) if isinstance(obst_multi_poly, MultiPolygon): obst_polys = obst_multi_poly.geoms @@ -180,7 +178,7 @@ from scenic.syntax.veneer import simulation behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.5, lookaheadTime=4, - vehBuffer=1, nonVehBuffer=0.2, erosionFactor=0.3): + vehBuffer=1, nonVehBuffer=0.2): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") @@ -211,7 +209,7 @@ behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, continue # Modify path to route around objects. - path_ls = getBugPath(self, path_ls, background_objects, bufferCalc, lookaheadTime) + path_ls = getBugPath(self, path_ls, background_objects, bufferCalc, lookaheadTime, vehBuffer, nonVehBuffer) self._planData = path_ls, targetSpeed # If path_ls is None, our goal is inside the danger zone and we can't From ed71d6936d917a2c91a03660eaba992324f8b9d5 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 14:51:36 -0700 Subject: [PATCH 089/134] Debug --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 15c42e816..9cba8c533 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -18,6 +18,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) # Compute the obstacle polygons, accounting for the plan of objects that have already logged it. + breakpoint() raw_obst_polys = [obj._boundingPolygon.buffer(baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer)) for obj in backgroundObjects] def future_poly_helper(obj): From de5f1e2d0caa02f4d4a4dca6e0fab946d83c7faf Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 14:58:14 -0700 Subject: [PATCH 090/134] Debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 9cba8c533..9618dccef 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -18,9 +18,13 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) # Compute the obstacle polygons, accounting for the plan of objects that have already logged it. - breakpoint() - raw_obst_polys = [obj._boundingPolygon.buffer(baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer)) - for obj in backgroundObjects] + raw_obst_polys = [] + for obj in backgroundObjects: + bufferAmount = baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer) + print(bufferAmount) + raw_obst_polys.append(obj._boundingPolygon.buffer(bufferAmount)) + # raw_obst_polys = [obj._boundingPolygon.buffer(baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer)) + # for obj in backgroundObjects] def future_poly_helper(obj): planned_path, planned_speed = obj._planData trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) From 1c43be831d8b5e6da8320452575376974ee617b0 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 14:59:42 -0700 Subject: [PATCH 091/134] Debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 9618dccef..eff05e666 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -28,7 +28,9 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh def future_poly_helper(obj): planned_path, planned_speed = obj._planData trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) - return trimmed_path.buffer(bufferCalc(obj) + shapely.minimum_bounding_radius(obj._boundingPolygon)) + bufferAmount = bufferCalc(obj) + shapely.minimum_bounding_radius(obj._boundingPolygon) + print(bufferAmount) + return trimmed_path.buffer(bufferAmount) future_polys = [future_poly_helper(obj) for obj in backgroundObjects if not obj.isVehicle and getattr(obj, "_planData", None) is not None] From 6e7bd1e1c3866885ddb233576166d76361834e85 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:00:40 -0700 Subject: [PATCH 092/134] Debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index eff05e666..92261b8db 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -28,7 +28,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh def future_poly_helper(obj): planned_path, planned_speed = obj._planData trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) - bufferAmount = bufferCalc(obj) + shapely.minimum_bounding_radius(obj._boundingPolygon) + bufferAmount = baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer) + shapely.minimum_bounding_radius(obj._boundingPolygon) print(bufferAmount) return trimmed_path.buffer(bufferAmount) future_polys = [future_poly_helper(obj) for obj in backgroundObjects From 6815416a42b2fce7a6a01bf620ab98dde189b264 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:02:36 -0700 Subject: [PATCH 093/134] More debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 92261b8db..3310c371b 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -16,21 +16,15 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh # Lambda to compute buffer const. baseBuffer = shapely.minimum_bounding_radius(actor._boundingPolygon) + bufferCalc = lambda obj: baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer) # Compute the obstacle polygons, accounting for the plan of objects that have already logged it. - raw_obst_polys = [] - for obj in backgroundObjects: - bufferAmount = baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer) - print(bufferAmount) - raw_obst_polys.append(obj._boundingPolygon.buffer(bufferAmount)) - # raw_obst_polys = [obj._boundingPolygon.buffer(baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer)) - # for obj in backgroundObjects] + raw_obst_polys = [obj._boundingPolygon.buffer(bufferCalc(obj)) + for obj in backgroundObjects] def future_poly_helper(obj): planned_path, planned_speed = obj._planData trimmed_path = shapely.ops.substring(planned_path, 0, planned_speed*lookaheadTime) - bufferAmount = baseBuffer + (vehBuffer if obj.isVehicle else nonVehBuffer) + shapely.minimum_bounding_radius(obj._boundingPolygon) - print(bufferAmount) - return trimmed_path.buffer(bufferAmount) + return trimmed_path.buffer(bufferCalc(obj) + + shapely.minimum_bounding_radius(obj._boundingPolygon)) future_polys = [future_poly_helper(obj) for obj in backgroundObjects if not obj.isVehicle and getattr(obj, "_planData", None) is not None] From a6875b62668ab3503771db776c4e2cb54bc14664 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:03:53 -0700 Subject: [PATCH 094/134] Remove debug. --- .../domains/driving/behaviors/walks.scenic | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 3310c371b..5a6990267 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -140,20 +140,20 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - from scenic.syntax.veneer import simulation - if simulation().currentRealTime > 4: - import matplotlib.pyplot as plt - plt.gca().set_aspect("equal") - from scenic.core.geometry import plotPolygon - from scenic.syntax.veneer import simulation - simulation().scene.workspace.network.show() - for obj in simulation().objects: - obj.show2D(simulation().scene.workspace, plt) - simulation().scene.workspace.zoomAround(plt, simulation().objects) - plotPolygon(obst_multi_poly, plt, style="c--") - plotPolygon(orig_path_ls, plt, style="y-") - plotPolygon(path_ls, plt, style="g--") - plt.show() + # from scenic.syntax.veneer import simulation + # if simulation().currentRealTime > 4: + # import matplotlib.pyplot as plt + # plt.gca().set_aspect("equal") + # from scenic.core.geometry import plotPolygon + # from scenic.syntax.veneer import simulation + # simulation().scene.workspace.network.show() + # for obj in simulation().objects: + # obj.show2D(simulation().scene.workspace, plt) + # simulation().scene.workspace.zoomAround(plt, simulation().objects) + # plotPolygon(obst_multi_poly, plt, style="c--") + # plotPolygon(orig_path_ls, plt, style="y-") + # plotPolygon(path_ls, plt, style="g--") + # plt.show() return path_ls From e580993c5bf80a3a64390e485d3fa863b932d6dd Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:05:35 -0700 Subject: [PATCH 095/134] Re-add debug. --- .../domains/driving/behaviors/walks.scenic | 32 ++++++++----------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 5a6990267..99decbcf0 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -140,20 +140,20 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - # from scenic.syntax.veneer import simulation - # if simulation().currentRealTime > 4: - # import matplotlib.pyplot as plt - # plt.gca().set_aspect("equal") - # from scenic.core.geometry import plotPolygon - # from scenic.syntax.veneer import simulation - # simulation().scene.workspace.network.show() - # for obj in simulation().objects: - # obj.show2D(simulation().scene.workspace, plt) - # simulation().scene.workspace.zoomAround(plt, simulation().objects) - # plotPolygon(obst_multi_poly, plt, style="c--") - # plotPolygon(orig_path_ls, plt, style="y-") - # plotPolygon(path_ls, plt, style="g--") - # plt.show() + from scenic.syntax.veneer import simulation + if simulation().currentRealTime > 4: + import matplotlib.pyplot as plt + plt.gca().set_aspect("equal") + from scenic.core.geometry import plotPolygon + from scenic.syntax.veneer import simulation + simulation().scene.workspace.network.show() + for obj in simulation().objects: + obj.show2D(simulation().scene.workspace, plt) + simulation().scene.workspace.zoomAround(plt, simulation().objects) + plotPolygon(obst_multi_poly, plt, style="c--") + plotPolygon(orig_path_ls, plt, style="y-") + plotPolygon(path_ls, plt, style="g--") + plt.show() return path_ls @@ -173,10 +173,6 @@ behavior _WalkPathHelper(path, targetSpeed): heading = angle from self to target_pt take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) -import matplotlib.pyplot as plt -from scenic.core.geometry import plotPolygon -from scenic.syntax.veneer import simulation - behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.5, lookaheadTime=4, vehBuffer=1, nonVehBuffer=0.2): From 51bceb54536a68653d9d150c16cec96b8de6eb93 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:09:13 -0700 Subject: [PATCH 096/134] More debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 99decbcf0..92b05643f 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -133,6 +133,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh # If the closest point on the mid path is very close, cut start path short # and aim directly for it. This helps avoid backtracking loop. if self_pt.distance(mid_path) < 1: + print("TRUNCATING!!!") mid_path = shapely.ops.substring(mid_path, mid_path.project(self_pt, normalized=True), 1, normalized=True) start_pt = ShapelyPoint(mid_path.coords[0]) start_path = LineString([self_pt, start_pt]) From dbb0b8b8347bf37d625d47ebe073f48c1f80a376 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:12:31 -0700 Subject: [PATCH 097/134] More debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 92b05643f..5ce228e6c 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -142,7 +142,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh path_ls = shapely.remove_repeated_points(path_ls) from scenic.syntax.veneer import simulation - if simulation().currentRealTime > 4: + if simulation().currentRealTime > 8: import matplotlib.pyplot as plt plt.gca().set_aspect("equal") from scenic.core.geometry import plotPolygon From aaedddb1446801708deaa4b8f5cd5df04f1194d5 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:17:24 -0700 Subject: [PATCH 098/134] More debug. --- src/scenic/domains/driving/behaviors/walks.scenic | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 5ce228e6c..4222d3b8f 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -133,7 +133,6 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh # If the closest point on the mid path is very close, cut start path short # and aim directly for it. This helps avoid backtracking loop. if self_pt.distance(mid_path) < 1: - print("TRUNCATING!!!") mid_path = shapely.ops.substring(mid_path, mid_path.project(self_pt, normalized=True), 1, normalized=True) start_pt = ShapelyPoint(mid_path.coords[0]) start_path = LineString([self_pt, start_pt]) @@ -142,7 +141,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh path_ls = shapely.remove_repeated_points(path_ls) from scenic.syntax.veneer import simulation - if simulation().currentRealTime > 8: + if path_ls.intersects(obst_multi_poly.buffer(-0.5)):#simulation().currentRealTime > 8: import matplotlib.pyplot as plt plt.gca().set_aspect("equal") from scenic.core.geometry import plotPolygon @@ -155,6 +154,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh plotPolygon(orig_path_ls, plt, style="y-") plotPolygon(path_ls, plt, style="g--") plt.show() + breakpoint() return path_ls From 8af29fdbd253edf14e8f91aa55f51f18e862ba3b Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:20:37 -0700 Subject: [PATCH 099/134] debug --- src/scenic/domains/driving/behaviors/walks.scenic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 4222d3b8f..3ade68105 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -153,7 +153,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh plotPolygon(obst_multi_poly, plt, style="c--") plotPolygon(orig_path_ls, plt, style="y-") plotPolygon(path_ls, plt, style="g--") - plt.show() + plt.show(block=False) breakpoint() return path_ls From 27d4f3af6ed50d49204602df5ecf001b12cf20b2 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:24:36 -0700 Subject: [PATCH 100/134] Fixed bug. --- src/scenic/domains/driving/behaviors/walks.scenic | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 3ade68105..6f9fb37b4 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -126,8 +126,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh mid_path = shapely.force_2d(mid_path) # Reverse the mid path if needed. - if (ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(start_path.coords[0])) - > ShapelyPoint(mid_path.coords[0]).distance(ShapelyPoint(end_path.coords[0]))): + if (ShapelyPoint(mid_path.coords[0]).distance(start_pt) > ShapelyPoint(mid_path.coords[0]).distance(end_pt)): mid_path = mid_path.reverse() # If the closest point on the mid path is very close, cut start path short From 211a706d0b1049fdc5ba799f26e114ddbaac2a80 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:30:45 -0700 Subject: [PATCH 101/134] Remove debug and tweaked params. --- .../domains/driving/behaviors/walks.scenic | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 6f9fb37b4..a2cdfac0c 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -139,21 +139,21 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) - from scenic.syntax.veneer import simulation - if path_ls.intersects(obst_multi_poly.buffer(-0.5)):#simulation().currentRealTime > 8: - import matplotlib.pyplot as plt - plt.gca().set_aspect("equal") - from scenic.core.geometry import plotPolygon - from scenic.syntax.veneer import simulation - simulation().scene.workspace.network.show() - for obj in simulation().objects: - obj.show2D(simulation().scene.workspace, plt) - simulation().scene.workspace.zoomAround(plt, simulation().objects) - plotPolygon(obst_multi_poly, plt, style="c--") - plotPolygon(orig_path_ls, plt, style="y-") - plotPolygon(path_ls, plt, style="g--") - plt.show(block=False) - breakpoint() + # from scenic.syntax.veneer import simulation + # if path_ls.intersects(obst_multi_poly.buffer(-0.5)):#simulation().currentRealTime > 8: + # import matplotlib.pyplot as plt + # plt.gca().set_aspect("equal") + # from scenic.core.geometry import plotPolygon + # from scenic.syntax.veneer import simulation + # simulation().scene.workspace.network.show() + # for obj in simulation().objects: + # obj.show2D(simulation().scene.workspace, plt) + # simulation().scene.workspace.zoomAround(plt, simulation().objects) + # plotPolygon(obst_multi_poly, plt, style="c--") + # plotPolygon(orig_path_ls, plt, style="y-") + # plotPolygon(path_ls, plt, style="g--") + # plt.show(block=False) + # breakpoint() return path_ls @@ -174,8 +174,8 @@ behavior _WalkPathHelper(path, targetSpeed): take SetWalkingDirectionAction(heading), SetWalkingSpeedAction(actual_speed) behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, - terminationThresh=0.1, replanTime=0.5, lookaheadTime=4, - vehBuffer=1, nonVehBuffer=0.2): + terminationThresh=0.1, replanTime=0.1, lookaheadTime=4, + vehBuffer=1.5, nonVehBuffer=0.2): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") From 68b37d5c8969dd2d875c1bab7eadecb4ff3b0c31 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 15:34:30 -0700 Subject: [PATCH 102/134] Added assertion --- src/scenic/domains/driving/behaviors/walks.scenic | 1 + 1 file changed, 1 insertion(+) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index a2cdfac0c..0cc159ef4 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -123,6 +123,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh else: exterior_segments.sort(key=lambda x: x.length) mid_path = exterior_segments[0] + assert isinstance(mid_path, LineString) mid_path = shapely.force_2d(mid_path) # Reverse the mid path if needed. From b6ea999deaab34a734660a920a99badbc3ef1fac Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 15 Jul 2026 16:17:54 -0700 Subject: [PATCH 103/134] Slightly increased vehicle buffer amount. --- src/scenic/domains/driving/behaviors/walks.scenic | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 0cc159ef4..2ee72364a 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -116,7 +116,7 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh # Extract the mid_path. If paths are very close in length, # bias to the right. # TODO: Bias to the appropriate driving direction - if 0.7 < exterior_segments[0].length/exterior_segments[1].length < 1.3: + if 0.95 < exterior_segments[0].length/exterior_segments[1].length < 1.05: def angle_helper(ls): return actor.apparentHeadingTo(Vector(*ls.centroid.coords[0])) exterior_segments.sort(key=lambda x: angle_helper(x)) @@ -176,7 +176,7 @@ behavior _WalkPathHelper(path, targetSpeed): behavior WalkPath(path, targetSpeed, *, avoidObstacles=True, terminationThresh=0.1, replanTime=0.1, lookaheadTime=4, - vehBuffer=1.5, nonVehBuffer=0.2): + vehBuffer=2, nonVehBuffer=0.2): """ Walk a path at targetSpeed, stopping at the end.""" if not isinstance(path, PolylineRegion): raise ValueError("`path` must be a `PolylineRegion`.") From 24961b55443eaac54bf5396639ea3240c944e461 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 16 Jul 2026 12:14:28 -0700 Subject: [PATCH 104/134] Add show2D to dynamicScenario. --- src/scenic/core/dynamics/scenarios.py | 15 +++++++++++++++ src/scenic/core/requirements.py | 1 + tests/syntax/test_dynamics.py | 20 ++++++++++++++++++++ tests/syntax/test_imports.py | 1 + 4 files changed, 37 insertions(+) diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index af22fd990..f8f84e168 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -566,6 +566,21 @@ def __str__(self): args = argsToString(self._args, self._kwargs) return f"{self.__class__.__name__}({args})" + def show2D(self, zoom=None, block=True): + """Render a 2D schematic of the scene for debugging.""" + import matplotlib.pyplot as plt + + plt.gca().set_aspect("equal") + # display map + self._workspace.show2D(plt) + # draw objects + for obj in self._objects: + obj.show2D(self._workspace, plt, highlight=(obj is self._ego)) + # zoom in if requested + if zoom: + self._workspace.zoomAround(plt, self._objects, expansion=zoom) + plt.show(block=block) + class LocalsSnapshot(Samplable): def __init__(self, locs): diff --git a/src/scenic/core/requirements.py b/src/scenic/core/requirements.py index 9c29b12e8..cf8f35c66 100644 --- a/src/scenic/core/requirements.py +++ b/src/scenic/core/requirements.py @@ -27,6 +27,7 @@ class RequirementType(enum.Enum): monitor = "require monitor" terminateWhen = "terminate when" terminateSimulationWhen = "terminate simulation when" + terminateAfter = "terminate after" # recorded values, which aren't requirements but are handled similarly record = "record" diff --git a/tests/syntax/test_dynamics.py b/tests/syntax/test_dynamics.py index 3a7bd3c43..a2489cfb4 100644 --- a/tests/syntax/test_dynamics.py +++ b/tests/syntax/test_dynamics.py @@ -576,6 +576,26 @@ def test_terminate_when(): assert tuple(actions) == (1, 2) +def test_terminate_minimum_time(): + scenario = compileScenic( + """ + behavior Foo(): + i = 0 + while True: + i += 1 + take i + ego = new Object with behavior Foo + terminate after 6 seconds + terminate after 4 steps + terminate after 5 steps + """ + ) + actions = sampleEgoActions(scenario, maxSteps=3) + assert tuple(actions) == (1, 2, 3) + actions = sampleEgoActions(scenario, maxSteps=7) + assert tuple(actions) == (1, 2, 3, 4) + + # Reuse diff --git a/tests/syntax/test_imports.py b/tests/syntax/test_imports.py index af519c9a6..88b3551f1 100644 --- a/tests/syntax/test_imports.py +++ b/tests/syntax/test_imports.py @@ -98,6 +98,7 @@ def test_inherit_terminate(runLocally): import helper_terminate ego = new Object record 1 as "foo" + terminate after 4 steps """ ) From 99ec36dc280b79e373a464dc0a972f0905bfcebd Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 16 Jul 2026 13:55:52 -0700 Subject: [PATCH 105/134] Tweaks. --- src/scenic/core/geometry.py | 6 +++- src/scenic/core/regions.py | 5 ++-- .../domains/driving/behaviors/walks.scenic | 30 +++++++++++-------- 3 files changed, 25 insertions(+), 16 deletions(-) diff --git a/src/scenic/core/geometry.py b/src/scenic/core/geometry.py index b60a68b2d..343f08477 100644 --- a/src/scenic/core/geometry.py +++ b/src/scenic/core/geometry.py @@ -2,6 +2,7 @@ import itertools import math +from typing import Iterable import warnings import numpy as np @@ -14,6 +15,7 @@ needsSampling, ) from scenic.core.lazy_eval import isLazy +from scenic.core.type_support import toVector from scenic.core.utils import cached_property @@ -110,7 +112,9 @@ def distanceToLine(point, a, b): # Fastest known way to make a Shapely Point from a list/tuple/Vector -makeShapelyPoint = shapely.points +makeShapelyPoint = lambda pt: ( + shapely.points(pt) if isinstance(pt, Iterable) else shapely.points(toVector(pt)) +) def polygonUnion(polys, buf=0, tolerance=0, holeTolerance=0.002): diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index c7971de2c..e96dda081 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -11,6 +11,7 @@ import itertools import math import random +from typing import Iterable import warnings import fcl @@ -720,8 +721,8 @@ def toPolygon(thing): poly = thing.polygons elif hasattr(thing, "lineString"): poly = thing.lineString - elif isinstance(thing, Vector): - poly = shapely.Point(*thing) + elif isinstance(thing, (Iterable, Vector)): + poly = makeShapelyPoint(thing) else: return None diff --git a/src/scenic/domains/driving/behaviors/walks.scenic b/src/scenic/domains/driving/behaviors/walks.scenic index 2ee72364a..8031d5248 100644 --- a/src/scenic/domains/driving/behaviors/walks.scenic +++ b/src/scenic/domains/driving/behaviors/walks.scenic @@ -123,19 +123,23 @@ def getBugPath(actor, path_ls, backgroundObjects, bufferCalc, lookaheadTime, veh else: exterior_segments.sort(key=lambda x: x.length) mid_path = exterior_segments[0] - assert isinstance(mid_path, LineString) - mid_path = shapely.force_2d(mid_path) - - # Reverse the mid path if needed. - if (ShapelyPoint(mid_path.coords[0]).distance(start_pt) > ShapelyPoint(mid_path.coords[0]).distance(end_pt)): - mid_path = mid_path.reverse() - - # If the closest point on the mid path is very close, cut start path short - # and aim directly for it. This helps avoid backtracking loop. - if self_pt.distance(mid_path) < 1: - mid_path = shapely.ops.substring(mid_path, mid_path.project(self_pt, normalized=True), 1, normalized=True) - start_pt = ShapelyPoint(mid_path.coords[0]) - start_path = LineString([self_pt, start_pt]) + if isinstance(mid_path, LineString): + mid_path = shapely.force_2d(mid_path) + + # Reverse the mid path if needed. + if (ShapelyPoint(mid_path.coords[0]).distance(start_pt) > ShapelyPoint(mid_path.coords[0]).distance(end_pt)): + mid_path = mid_path.reverse() + + # If the closest point on the mid path is very close, cut start path short + # and aim directly for it. This helps avoid backtracking loop. + if self_pt.distance(mid_path) < 1: + mid_path = shapely.ops.substring(mid_path, mid_path.project(self_pt, normalized=True), 1, normalized=True) + start_pt = ShapelyPoint(mid_path.coords[0]) + start_path = LineString([self_pt, start_pt]) + elif isinstance(mid_path, ShapelyPoint): + continue + else: + assert False, mid_path path_ls = LineString(list(start_path.coords) + list(mid_path.coords) + list(end_path.coords)) path_ls = shapely.remove_repeated_points(path_ls) From fbc9ae51b9853c58f372d1f5e44d3e61eb99289b Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 16 Jul 2026 14:01:04 -0700 Subject: [PATCH 106/134] Added center property to region --- src/scenic/core/regions.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index e96dda081..a4e3eb990 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -124,6 +124,10 @@ def AABB(self): """Axis-aligned bounding box for this `Region`.""" pass + @cached_property + def center(self): + Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 + ## Overridable Methods ## # The following methods can be overriden to get better performance or if the region # has dependencies (in the case of sampleGiven). From 998a299cf0c4f9c2709c9fd1d15dbd20b16656dd Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 16 Jul 2026 14:01:52 -0700 Subject: [PATCH 107/134] Fix. --- src/scenic/core/regions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index a4e3eb990..f4cdf2089 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -126,7 +126,7 @@ def AABB(self): @cached_property def center(self): - Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 + return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 ## Overridable Methods ## # The following methods can be overriden to get better performance or if the region From 2b82b2d5d9dbbc29066df67d4af942e1a9d506c0 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Thu, 16 Jul 2026 14:34:10 -0700 Subject: [PATCH 108/134] Rename center to midpoint to avoid conflict with center. --- src/scenic/core/regions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index f4cdf2089..6e8e82292 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -125,7 +125,7 @@ def AABB(self): pass @cached_property - def center(self): + def midpoint(self): return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 ## Overridable Methods ## From 66f24aae8d25b3be49185ffcd93e823bacdc93b0 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 17 Jul 2026 09:56:18 -0700 Subject: [PATCH 109/134] Partial progress on terminate after changes. --- src/scenic/core/dynamics/scenarios.py | 2 ++ src/scenic/syntax/compiler.py | 28 +++++++--------- src/scenic/syntax/veneer.py | 25 ++++++++++----- tests/syntax/test_compiler.py | 46 ++++++++++++++++++++++++--- 4 files changed, 73 insertions(+), 28 deletions(-) diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index f8f84e168..7b14757fe 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -497,6 +497,8 @@ def _registerCompiledRequirement(self, req): place = self._terminationConditions elif req.ty is RequirementType.terminateSimulationWhen: place = self._terminateSimulationConditions + elif req.ty is RequirementType.terminateAfter: + place = self._terminationConditions elif req.ty is RequirementType.record: place = self._recordedExprs elif req.ty is RequirementType.recordInitial: diff --git a/src/scenic/syntax/compiler.py b/src/scenic/syntax/compiler.py index 551c8a3a7..2dbad82a2 100644 --- a/src/scenic/syntax/compiler.py +++ b/src/scenic/syntax/compiler.py @@ -1393,6 +1393,18 @@ def visit_TerminateSimulationWhen(self, node: s.TerminateSimulationWhen): "terminate_simulation_when", node.cond, node.lineno, node.name ) + @context(Context.TOP_LEVEL) + def visit_TerminateAfter(self, node: s.TerminateAfter): + cond = ast.Call( + func=ast.Name(id="check_time", ctx=loadCtx), + args=[ + self.visit(node.duration.value), + ast.Constant(node.duration.unitStr), + ], + keywords=[], + ) + return self.createRequirementLike("terminate_after", cond, node.lineno, None) + def createRequirementLike( self, functionName: str, @@ -1435,22 +1447,6 @@ def createRequirementLike( ) ) - @context(Context.TOP_LEVEL) - def visit_TerminateAfter(self, node: s.TerminateAfter): - return ast.copy_location( - ast.Expr( - ast.Call( - func=ast.Name(id="terminate_after", ctx=loadCtx), - args=[ - self.visit(node.duration.value), - ast.Constant(node.duration.unitStr), - ], - keywords=[], - ) - ), - node, - ) - @context(Context.TOP_LEVEL) def visit_Simulator(self, node: s.Simulator): return ast.copy_location( diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index ed2a89065..ab381409e 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -26,6 +26,7 @@ "terminate_when", "terminate_simulation_when", "terminate_after", + "check_time", "in_initial_scenario", "override", "record", @@ -876,6 +877,22 @@ def terminate_simulation_when(reqID, req, line, name): ) +def terminate_after(reqId, req, line, _): + name = "terminate after on line {line}" + makeRequirement(requirements.RequirementType.terminateAfter, reqId, req, line, name) + + +def check_time(timeLimit, terminator=None): + """Returns True if we have exceeded the time limit.""" + if not isinstance(timeLimit, (builtins.float, builtins.int)): + raise TypeError('"terminate after N" with N not a number') + assert terminator in (None, "seconds", "steps") + inSeconds = terminator != "steps" + + threshold = timeLimit / simulation().timestep if inSeconds else timeLimit + return simulation().currentTime >= threshold + + def makeRequirement(ty, reqID, req, line, name, recConfig=None): if evaluatingRequirement: raise InvalidScenarioError(f'tried to use "{ty.value}" inside a requirement') @@ -887,14 +904,6 @@ def makeRequirement(ty, reqID, req, line, name, recConfig=None): currentScenario._addRequirement(ty, reqID, req, line, name, 1, recConfig) -def terminate_after(timeLimit, terminator=None): - if not isinstance(timeLimit, (builtins.float, builtins.int)): - raise TypeError('"terminate after N" with N not a number') - assert terminator in (None, "seconds", "steps") - inSeconds = terminator != "steps" - currentScenario._setTimeLimit(timeLimit, inSeconds=inSeconds) - - def resample(dist): """The built-in resample function.""" if not isinstance(dist, Distribution): diff --git a/tests/syntax/test_compiler.py b/tests/syntax/test_compiler.py index 2a35dd121..ceac75760 100644 --- a/tests/syntax/test_compiler.py +++ b/tests/syntax/test_compiler.py @@ -1710,20 +1710,58 @@ def test_terminate_simulation_when(self): assert False def test_terminate_after_seconds(self): - node, _ = compileScenicAST(TerminateAfter(Seconds(Constant(10)))) + node, _ = compileScenicAST(TerminateAfter(Seconds(Constant(10)), lineno=2)) match node: case Expr( - Call(Name("terminate_after"), [Constant(10), Constant("seconds")], []) + Call( + Name("terminate_after"), + [ + Constant(0), # reqId + Call( + Name("AtomicProposition"), + [ + Lambda( + arguments(), + Call( + Name("check_time"), + [Constant(10), Constant("seconds")], + ), + ) + ], + ), + Constant(2), # lineno + Constant(None), # name + ], + ) ): assert True case _: assert False def test_terminate_after_steps(self): - node, _ = compileScenicAST(TerminateAfter(Steps(Constant(20)))) + node, _ = compileScenicAST(TerminateAfter(Steps(Constant(20)), lineno=2)) match node: case Expr( - Call(Name("terminate_after"), [Constant(20), Constant("steps")], []) + Call( + Name("terminate_after"), + [ + Constant(0), # reqId + Call( + Name("AtomicProposition"), + [ + Lambda( + arguments(), + Call( + Name("check_time"), + [Constant(20), Constant("steps")], + ), + ) + ], + ), + Constant(2), # lineno + Constant(None), # name + ], + ) ): assert True case _: From 8ab487a5d7665d06abdc6fe94eac905cb2a98758 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 17 Jul 2026 12:54:58 -0700 Subject: [PATCH 110/134] Inheritence almost fully working, modulo some questions of scenario scope. --- src/scenic/core/dynamics/scenarios.py | 6 +++--- src/scenic/core/simulators.py | 8 +++++++- src/scenic/syntax/veneer.py | 2 +- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index 8192ab7a3..59c5498d9 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -446,9 +446,9 @@ def _inherit(self, other): self._temporalRequirements.extend(other._temporalRequirements) self._terminationConditions.extend(other._terminationConditions) self._terminateSimulationConditions.extend(other._terminateSimulationConditions) - # self._recordedExprs.extend(other._recordedExprs) - # self._recordedInitialExprs.extend(other._recordedInitialExprs) - # self._recordedFinalExprs.extend(other._recordedFinalExprs) + self._recordedExprs.extend(other._recordedExprs) + self._recordedInitialExprs.extend(other._recordedInitialExprs) + self._recordedFinalExprs.extend(other._recordedFinalExprs) def _registerInstance(self, inst): self._instances.append(inst) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index c519ed3a0..50177e5ce 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -457,7 +457,13 @@ def _run(self, dynamicScenario, maxSteps): return terminationType, terminationReason terminationReason = dynamicScenario._checkSimulationTerminationConditions() if terminationReason is not None: - return TerminationType.simulationTerminationCondition, terminationReason + if terminationReason.ty is RequirementType.terminateAfter: + return TerminationType.timeLimit, terminationReason + else: + return ( + TerminationType.simulationTerminationCondition, + terminationReason, + ) if maxSteps and self.currentTime >= maxSteps: return TerminationType.timeLimit, f"reached time limit ({maxSteps} steps)" diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index a29645217..7d29712ba 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -870,7 +870,7 @@ def check_time(timeLimit, terminator=None): inSeconds = terminator != "steps" threshold = timeLimit / simulation().timestep if inSeconds else timeLimit - return simulation().currentTime >= threshold + return currentScenario._elapsedTime > threshold def makeRequirement(ty, reqID, req, line, name, recConfig=None): From 5d36fe81e5b5997f4f969bb19a8eea459c63bbc3 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 17 Jul 2026 14:34:07 -0700 Subject: [PATCH 111/134] New closestPointTo function --- src/scenic/core/regions.py | 75 +++++++++++++++++++++++++++++++++-- src/scenic/core/vectors.py | 8 +++- src/scenic/core/workspaces.py | 5 ++- 3 files changed, 81 insertions(+), 7 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 6e8e82292..0c69a0e84 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -118,16 +118,17 @@ def projectVector(self, point, onDirection): """Returns point projected onto this region along onDirection.""" pass + @abstractmethod + def closestPointTo(self, target): + """Returns the closest point to target (also a point) that is contained in the region""" + pass + @property @abstractmethod def AABB(self): """Axis-aligned bounding box for this `Region`.""" pass - @cached_property - def midpoint(self): - return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 - ## Overridable Methods ## # The following methods can be overriden to get better performance or if the region # has dependencies (in the case of sampleGiven). @@ -267,6 +268,10 @@ def __repr__(self): s += f" {self.name}" return s + f" at {hex(id(self))}>" + @cached_property + def midpoint(self): + return Vector(*self.AABB[0]) + Vector(*self.AABB[1]) / 2 + class PointInRegionDistribution(VectorDistribution): """Uniform distribution over points in a Region""" @@ -335,6 +340,9 @@ def distanceTo(self, point): def projectVector(self, point, onDirection): return point + def closestPointTo(self, target): + return toVector(target) + @property def AABB(self): raise TypeError("AllRegion does not have a well defined AABB") @@ -387,6 +395,9 @@ def distanceTo(self, point): def projectVector(self, point, onDirection): raise RejectionException("Projecting vector onto empty Region") + def closestPointTo(self, target): + raise RejectionException("Finding closest point in empty Region") + @property def AABB(self): raise TypeError("EmptyRegion does not have a well defined AABB") @@ -474,6 +485,9 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + def closestPointTo(self, target): + raise NotImplementedError + @property def AABB(self): raise NotImplementedError @@ -587,6 +601,12 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + def closestPointTo(self, target): + target = toVector(target) + candidate_points = [region.closestPointTo(target) for region in self.regions] + candidate_points.sort(key=lambda pt: pt.distanceTo(target)) + return candidate_points[0] + @property def AABB(self): raise NotImplementedError @@ -688,6 +708,9 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + def closestPointTo(self, target): + raise NotImplementedError + @property def AABB(self): raise NotImplementedError @@ -1010,6 +1033,10 @@ def projectVector(self, point, onDirection): return Vector(*closest_point) + @distributionFunction + def closestPointTo(self, target): + return toVector(trimesh.proximity.closest_point(self.mesh, target)) + @cached_property @distributionFunction def circumcircle(self): @@ -1769,6 +1796,15 @@ def distanceTo(self, point): return abs(dist) + @distributionFunction + def closestPointTo(self, target): + target = toVector(target) + + if self.containsPoint(target): + return target + + return super().closestPointTo(target) + @distributionFunction def minimumDistanceTo(self, other): """Get the minimum distance between this region and another. @@ -2315,6 +2351,9 @@ def distanceTo(self, point): def projectVector(self, point, onDirection): raise NotImplementedError + def closestPointTo(self, target): + raise NotImplementedError + def uniformPointInner(self): # First generate a point uniformly in a box with dimensions # equal to scale, centered at the origin. @@ -2605,6 +2644,12 @@ def projectVector(self, point, onDirection): f'{type(self).__name__} does not yet support projection using "on"' ) + @distributionFunction + def closestPointTo(self, target): + target = toVector(target) + pt_2d = toVector(shapely.ops.nearest_points(self.polygons, target)[0]) + return Vector(pt_2d.x, pt_2d.y, target.z) + @property def AABB(self): raise NotImplementedError @@ -2844,6 +2889,9 @@ def defaultOrientation(self, point): def projectVector(self, point, onDirection): raise NotImplementedError + def closestPointTo(self, target): + raise NotImplementedError + @cached_property def AABB(self): return ( @@ -3140,6 +3188,12 @@ def distanceTo(self, point): dist2D = shapely.distance(self.polygons, makeShapelyPoint(point)) return math.hypot(dist2D, point[2] - self.z) + @distributionFunction + def closestPointTo(self, target): + target = toVector(target) + pt_2d = toVector(shapely.ops.nearest_points(self.polygons, target)[0]) + return Vector(pt_2d.x, pt_2d.y, self.z) + @cached_property @distributionFunction def inradius(self): @@ -3731,6 +3785,10 @@ def distanceTo(self, point) -> float: dist2D = self.lineString.distance(makeShapelyPoint(point)) return math.hypot(dist2D, point.z) + @distributionMethod + def closestPointTo(self, target): + return toVector(shapely.ops.nearest_points(self.lineString, target)[0]) + def projectVector(self, point, onDirection): raise TypeError('PolylineRegion does not support projection using "on"') @@ -3958,6 +4016,12 @@ def distanceTo(self, point): distance, _ = self.kdTree.query(point) return distance + @distributionMethod + def closestPointTo(self, target): + point = toVector(point).coordinates + _, neighbor_i = self.kdTree.query(point) + return toVector(self.points[neighbor_i]) + def projectVector(self, point, onDirection): raise TypeError('PointSetRegion does not support projection using "on"') @@ -4082,6 +4146,9 @@ def containsRegionInner(self, reg, tolerance): def projectVector(self, point, onDirection): raise TypeError('GridRegion does not support projection using "on"') + def closestPointTo(self, target): + raise NotImplementedError + ################################################################################################### # View Regions diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index 83c640cfe..4b399df72 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -16,7 +16,7 @@ import numpy from scipy.spatial.transform import Rotation -import shapely.geometry +import shapely from scenic.core.distributions import ( Distribution, @@ -450,7 +450,9 @@ def toVector(self) -> Vector: @staticmethod def _canCoerceType(ty): - return issubclass(ty, (tuple, list, numpy.ndarray)) or hasattr(ty, "toVector") + return issubclass(ty, (tuple, list, numpy.ndarray, shapely.Point)) or hasattr( + ty, "toVector" + ) @staticmethod def _coerce(thing) -> Vector: @@ -461,6 +463,8 @@ def _coerce(thing) -> Vector: "expected 2D/3D vector, got " f"{type(thing).__name__} of length {l}" ) return Vector(*thing) + elif isinstance(thing, shapely.Point): + return Vector(*thing.coords[0]) else: return thing.toVector() diff --git a/src/scenic/core/workspaces.py b/src/scenic/core/workspaces.py index 8daec9aab..f3be2b943 100644 --- a/src/scenic/core/workspaces.py +++ b/src/scenic/core/workspaces.py @@ -117,7 +117,10 @@ def distanceTo(self, point): return self.region.distanceTo(point) def projectVector(self, point, onDirection): - raise self.region.projectVector(point, onDirection) + return self.region.projectVector(point, onDirection) + + def closestPointTo(self, target): + return self.region.closestPointTo(target) @property def AABB(self): From 6d2fdf6541d5aeb1ab7bf7ee899a9202ecf8d831 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 17 Jul 2026 14:36:44 -0700 Subject: [PATCH 112/134] Properly coerce to shapely point in closestPointTo --- src/scenic/core/regions.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 0c69a0e84..90184af04 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -2647,7 +2647,7 @@ def projectVector(self, point, onDirection): @distributionFunction def closestPointTo(self, target): target = toVector(target) - pt_2d = toVector(shapely.ops.nearest_points(self.polygons, target)[0]) + pt_2d = toVector(shapely.ops.nearest_points(self.polygons, toShapely(target))[0]) return Vector(pt_2d.x, pt_2d.y, target.z) @property @@ -3191,7 +3191,7 @@ def distanceTo(self, point): @distributionFunction def closestPointTo(self, target): target = toVector(target) - pt_2d = toVector(shapely.ops.nearest_points(self.polygons, target)[0]) + pt_2d = toVector(shapely.ops.nearest_points(self.polygons, toShapely(target))[0]) return Vector(pt_2d.x, pt_2d.y, self.z) @cached_property From 950c89b84f03ed677438bc6e0ce74d88c41f4948 Mon Sep 17 00:00:00 2001 From: Daniel Fremont Date: Sat, 18 Jul 2026 21:07:45 -0700 Subject: [PATCH 113/134] fix `record ... to` statement --- src/scenic/core/dynamics/scenarios.py | 10 ++- src/scenic/core/sensors.py | 2 +- src/scenic/syntax/veneer.py | 2 +- tests/syntax/test_dynamics.py | 1 + tests/syntax/test_recording.py | 110 ++++++++++++++++++++++++++ 5 files changed, 120 insertions(+), 5 deletions(-) create mode 100644 tests/syntax/test_recording.py diff --git a/src/scenic/core/dynamics/scenarios.py b/src/scenic/core/dynamics/scenarios.py index d931e9587..61ee09254 100644 --- a/src/scenic/core/dynamics/scenarios.py +++ b/src/scenic/core/dynamics/scenarios.py @@ -387,15 +387,19 @@ def _updateRecords(self): def _recordTimeSeries(self): from scenic.syntax.veneer import currentSimulation - if self._recordedTime == currentSimulation.currentTime: + currentTime = currentSimulation.currentTime + if self._recordedTime == currentTime: # This time step was already recorded (e.g. the scenario was terminated # by a behavior after the current state was recorded). return for rec in self._recordedExprs: - currentSimulation._recordTimeSeries(rec.name, rec.evaluate()) + value = rec.evaluate() + currentSimulation._recordTimeSeries(rec.name, value) + if (recConfig := rec.recConfig) and (recorder := recConfig.recorder): + recorder._record(value, currentTime) - self._recordedTime = currentSimulation.currentTime + self._recordedTime = currentTime def _runMonitors(self): terminationReason = None diff --git a/src/scenic/core/sensors.py b/src/scenic/core/sensors.py index c4805ea10..b11f0b7ae 100644 --- a/src/scenic/core/sensors.py +++ b/src/scenic/core/sensors.py @@ -266,7 +266,7 @@ def videoHandler(path, values, timestep, options): @fileHandler("npz") def npzHandler(path, values, timestep, options): - timesteps, values = zip(*values) + timesteps, values = zip(*values) if values else ([], []) np.savez_compressed(path, timesteps=timesteps, values=values) diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index 37994e21d..cb905e5b8 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -545,7 +545,7 @@ def executeInRequirement(scenario, boundEgo, values): except RandomControlFlowError as e: # Such errors should not be possible inside a requirement, since all values # should have already been sampled: something's gone wrong with our rebinding. - raise RuntimeError( + raise AssertionError( "internal error: requirement dependency not sampled" ) from e finally: diff --git a/tests/syntax/test_dynamics.py b/tests/syntax/test_dynamics.py index 4699a2921..11d3ef61d 100644 --- a/tests/syntax/test_dynamics.py +++ b/tests/syntax/test_dynamics.py @@ -2223,6 +2223,7 @@ def test_termination_reason_monitor(): ## Recording +# (see also `test_recording.py`) def test_record(): diff --git a/tests/syntax/test_recording.py b/tests/syntax/test_recording.py new file mode 100644 index 000000000..4c03cff60 --- /dev/null +++ b/tests/syntax/test_recording.py @@ -0,0 +1,110 @@ +"""Tests for advanced usages of the `record` statement.""" + +import numpy as np + +from tests.utils import compileScenic, sampleResult + +## Utilities + + +def checkRecordTo(tmp_path, period=None, delay=None, maxSteps=5): + """Helper for testing the `record ... to ...` statement. + + Returns the timesteps at which a value was recorded. + """ + + # Clear out the folder in case the helper is used multiple times in a test + for f in tmp_path.iterdir(): + f.unlink() + + every = f"every {period}" if period else "" + after = f"after {delay}" if delay else "" + scenario = compileScenic( + f""" + record -simulation().currentTime {every} {after} to "value_{{step}}.npy" + record -simulation().currentTime {every} {after} to "series.npz" + """, + params=dict(recordFolder=tmp_path), + ) + result = sampleResult(scenario, maxSteps=maxSteps) + assert result is not None + + recordedTimes = [] + for t in range(maxSteps + 1): + path = tmp_path / f"value_{t}.npy" + if path.exists(): + value = np.load(path) + assert float(value) == -t + recordedTimes.append(t) + + series = np.load(tmp_path / "series.npz") + assert np.array_equal(series["timesteps"], recordedTimes) + assert np.array_equal(series["values"], [-t for t in recordedTimes]) + + return recordedTimes + + +## Recording to files + + +def test_record_to(tmp_path): + times = checkRecordTo(tmp_path) + assert times == [0, 1, 2, 3, 4, 5] + + +def test_record_to_after(tmp_path): + times = checkRecordTo(tmp_path, delay="2 steps") + assert times == [2, 3, 4, 5] + + times = checkRecordTo(tmp_path, delay="3.5 seconds") + assert times == [3, 4, 5] + + times = checkRecordTo(tmp_path, delay="10 steps") + assert times == [] + + +def test_record_to_every(tmp_path): + times = checkRecordTo(tmp_path, period="2 steps") + assert times == [0, 2, 4] + + times = checkRecordTo(tmp_path, period="3.5 seconds") + assert times == [0, 3] + + times = checkRecordTo(tmp_path, period="10 steps") + assert times == [0] + + +def test_record_to_in_subscenario(tmp_path): + scenario = compileScenic( + """ + scenario Main(): + compose: + wait for 2 steps + do Sub(1) + wait for 2 steps + do Sub(2) + wait + scenario Sub(i): + record simulation().currentTime to "value_{step}.npy" + record simulation().currentTime to f"series{i}.npz" + terminate after 2 steps + """, + params=dict(recordFolder=tmp_path), + ) + result = sampleResult(scenario, maxSteps=10) + assert result is not None + + for t in range(11): + path = tmp_path / f"value_{t}.npy" + if 2 <= t <= 4 or 6 <= t <= 8: + value = np.load(path) + assert float(value) == t + else: + assert not path.exists(), t + + series1 = np.load(tmp_path / "series1.npz") + assert np.array_equal(series1["timesteps"], [2, 3, 4]) + assert np.array_equal(series1["values"], [2, 3, 4]) + series2 = np.load(tmp_path / "series2.npz") + assert np.array_equal(series2["timesteps"], [6, 7, 8]) + assert np.array_equal(series2["values"], [6, 7, 8]) From 069bec89d6fa9be5fb15ae9a179e606fbff3dfa9 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 21 Jul 2026 13:03:36 -0700 Subject: [PATCH 114/134] Simulation cleanup fix and tests. --- src/scenic/core/simulators.py | 3 +- tests/core/test_simulators.py | 98 ++++++++++++++++++++++++++++++++++- tests/utils.py | 23 +++++--- 3 files changed, 115 insertions(+), 9 deletions(-) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index 4baca26ef..ff9c2c1d4 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -278,7 +278,6 @@ def _runSingleSimulation( **kwargs, ) as simulation: simulation._run() - except (RejectSimulationException, RejectionException, GuardViolation) as e: if verbosity >= 2: print( @@ -437,7 +436,7 @@ def __init__( # properties during setup. self.updateObjects() - except (RejectSimulationException, RejectionException, GuardViolation) as e: + except Exception as e: # This simulation will be thrown out, but attach it to the exception # to aid in debugging. self.cleanup() diff --git a/tests/core/test_simulators.py b/tests/core/test_simulators.py index b3fb0e586..0e9d1d60e 100644 --- a/tests/core/test_simulators.py +++ b/tests/core/test_simulators.py @@ -11,7 +11,14 @@ SimulatorGroup, TerminatedSimulationException, ) -from tests.utils import compileScenic, sampleResultFromScene, sampleSceneFrom +from tests.utils import ( + RejectSimulationException, + checkVeneerIsInactive, + compileScenic, + sampleResult, + sampleResultFromScene, + sampleSceneFrom, +) def test_old_style_simulator(): @@ -221,3 +228,92 @@ def test_simulator_group_deterministic(): assert len(results1) == len(results2) assert all((v1 is None) == (v2 is None) for v1, v2 in zip(results1, results2)) + + +def test_simulator_createObjectInSimulator_error_cleanup(): + destroy_called = False + + class TestException(Exception): + pass + + class TestSimulator(DummySimulator): + def createSimulation(self, scene, **kwargs): + return TestSimulation(scene, drift=self.drift, **kwargs) + + class TestSimulation(DummySimulation): + def createObjectInSimulator(self, obj): + raise TestException() + + def destroy(self): + nonlocal destroy_called + destroy_called = True + + simulator = TestSimulator() + checkVeneerIsInactive() + with pytest.raises(TestException): + sampleResult(compileScenic("ego = new Object"), simulator=simulator) + checkVeneerIsInactive() + + assert destroy_called + + +def test_simulator_step_error_cleanup(): + destroy_called = False + + class TestException(Exception): + pass + + class TestSimulator(DummySimulator): + def createSimulation(self, scene, **kwargs): + return TestSimulation(scene, drift=self.drift, **kwargs) + + class TestSimulation(DummySimulation): + def step(self): + raise TestException() + + def destroy(self): + nonlocal destroy_called + destroy_called = True + + simulator = TestSimulator() + checkVeneerIsInactive() + with pytest.raises(TestException): + sampleResult(compileScenic("ego = new Object"), simulator=simulator) + checkVeneerIsInactive() + + assert destroy_called + + +def test_simulator_rejection_cleanup(): + destroy_called = False + + class TestException(Exception): + pass + + class TestSimulator(DummySimulator): + def createSimulation(self, scene, **kwargs): + return TestSimulation(scene, drift=self.drift, **kwargs) + + class TestSimulation(DummySimulation): + def destroy(self): + nonlocal destroy_called + destroy_called = True + + simulator = TestSimulator() + checkVeneerIsInactive() + with pytest.raises(RejectSimulationException): + sampleResult( + compileScenic( + """ + ego = new Object + monitor Foo(): + require False + wait + require monitor Foo() + """ + ), + simulator=simulator, + ) + checkVeneerIsInactive() + + assert destroy_called diff --git a/tests/utils.py b/tests/utils.py index d8b284fcf..8da9fc189 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -185,11 +185,17 @@ def sampleTrajectory( ) -def sampleResult(scenario, maxIterations=1, maxSteps=1, maxScenes=1, timestep=1): +def sampleResult( + scenario, maxIterations=1, maxSteps=1, maxScenes=1, timestep=1, simulator=None +): for i in range(maxScenes): scene, iterations = generateChecked(scenario, maxIterations) result = sampleResultFromScene( - scene, maxIterations=maxIterations, maxSteps=maxSteps, timestep=timestep + scene, + maxIterations=maxIterations, + maxSteps=maxSteps, + timestep=timestep, + simulator=simulator, ) if result is not None: return result @@ -198,16 +204,21 @@ def sampleResult(scenario, maxIterations=1, maxSteps=1, maxScenes=1, timestep=1) ) -def sampleResultOnce(scenario, maxSteps=1, timestep=1): +def sampleResultOnce(scenario, maxSteps=1, timestep=1, simulator=None): scene = sampleScene(scenario) - sim = DummySimulator() + sim = DummySimulator() if simulator is None else simulator return sim.simulate(scene, maxSteps=maxSteps, maxIterations=1, timestep=timestep) def sampleResultFromScene( - scene, maxIterations=1, maxSteps=1, raiseGuardViolations=False, timestep=1 + scene, + maxIterations=1, + maxSteps=1, + raiseGuardViolations=False, + timestep=1, + simulator=None, ): - sim = DummySimulator() + sim = DummySimulator() if simulator is None else simulator simulation = sim.simulate( scene, maxSteps=maxSteps, From f0a75b46d77b2a6efb54bb8a4554ffd02d473c9b Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 21 Jul 2026 13:23:21 -0700 Subject: [PATCH 115/134] Merge fixes. --- src/scenic/core/simulators.py | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/scenic/core/simulators.py b/src/scenic/core/simulators.py index c3155b155..39b32ebbf 100644 --- a/src/scenic/core/simulators.py +++ b/src/scenic/core/simulators.py @@ -474,8 +474,8 @@ def advance(self): {key: sensor.getObservation() for key, sensor in obj.sensors.items()} ) - # Record current state of the simulation - self._recordCurrentState() + # Record current state of the simulation + self._recordCurrentState() # Run monitors newReason = self.dynamicScenario._runMonitors() @@ -587,13 +587,6 @@ def terminateSimulation(self, terminationType, terminationReason): for scenario in tuple(reversed(veneer.runningScenarios)): scenario._stop("simulation terminated") - # Record finally-recorded values. - values = self.dynamicScenario._evaluateRecordedExprs( - RequirementType.recordFinal, self.currentTime - ) - for name, val in values.items(): - self.records[name] = val - # Package up simulation results into a compact object. result = SimulationResult( self.name, From 4092c514333293820d10f789304bec2932107b71 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 12:59:27 -0700 Subject: [PATCH 116/134] Experimental Optunal external sampler support. --- pyproject.toml | 1 + src/scenic/core/distributions.py | 17 +- src/scenic/core/external_params.py | 499 -------------------------- src/scenic/core/geometry.py | 5 + src/scenic/core/regions.py | 223 ++++++++++-- src/scenic/core/scenarios.py | 13 +- src/scenic/core/vectors.py | 2 + src/scenic/core/workspaces.py | 7 + src/scenic/simulators/utils/colors.py | 2 + src/scenic/syntax/translator.py | 41 ++- src/scenic/syntax/veneer.py | 21 +- 11 files changed, 297 insertions(+), 534 deletions(-) delete mode 100644 src/scenic/core/external_params.py diff --git a/pyproject.toml b/pyproject.toml index 14cfd99e6..e5c8fd8a1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -36,6 +36,7 @@ dependencies = [ "networkx >= 2.6", "numpy >= 1.24", "opencv-python ~= 4.5", + "optuna ~= 4.0", "pegen >= 0.3.0", "pillow >= 9.1", 'pygame-ce >= 2.5.7, <3; python_version >= "3.10"', diff --git a/src/scenic/core/distributions.py b/src/scenic/core/distributions.py index f31165985..c522b56ec 100644 --- a/src/scenic/core/distributions.py +++ b/src/scenic/core/distributions.py @@ -126,6 +126,7 @@ def __init__(self, dependencies): props.update(requiredProperties(dep)) super().__init__(props, deps) self._conditioned = self # version (partially) conditioned on requirements + self._conditionTarget = False @staticmethod def sampleAll(quantities): @@ -172,6 +173,8 @@ def deserializeValue(self, serializer, values): def conditionTo(self, value): """Condition this value to another value with the same conditional distribution.""" + assert not self._conditionTarget, "Attempted to double condition!" + value._conditionTarget = True assert isinstance(value, Samplable) self._conditioned = value @@ -182,6 +185,17 @@ def evaluateIn(self, context): assert all(not needsLazyEvaluation(dep) for dep in value._dependencies) return value + def recursiveDependencies(self): + self = self._conditioned + deps = set( + itertools.chain.from_iterable( + dep.recursiveDependencies() for dep in self._dependencies + ) + ) + if isinstance(self, Distribution) and not self._deterministic: + deps.add(self) + return deps + class ConstantSamplable(Samplable): """A samplable which always evaluates to a constant value. @@ -1076,6 +1090,7 @@ def cdf(mean, stddev, x): return (1 + math.erf((x - mean) / (sqrt2 * stddev))) / 2 @staticmethod + @distributionFunction def cdfinv(mean, stddev, x): import scipy # slow import not often needed @@ -1271,7 +1286,7 @@ def supportInterval(self): def __repr__(self): weights = self.weights - if all(weight == weights[0] for weight in weights): + if not weights or all(weight == weights[0] for weight in weights): return f"DiscreteRange({self.low!r}, {self.high!r})" else: return f"DiscreteRange({self.low!r}, {self.high!r}, {self.weights})" diff --git a/src/scenic/core/external_params.py b/src/scenic/core/external_params.py deleted file mode 100644 index 211cf22a9..000000000 --- a/src/scenic/core/external_params.py +++ /dev/null @@ -1,499 +0,0 @@ -"""Support for values which are sampled outside of Scenic. - -External Samplers in General -============================ - -External samplers provide a mechanism to use different types of sampling -techniques, like optimization or quasi-random sampling, from within a Scenic -program. Ordinary random values in Scenic are instances of `Distribution`; -this module defines a special subclass, `ExternalParameter`, representing a -value which is sampled externally. Scenic programs with external parameters -are handled as follows: - - 1. During compilation, all instances of `ExternalParameter` are gathered - together and given to the `ExternalSampler.forParameters` function; - this function creates an appropriate `ExternalSampler`, - whose configuration can be controlled using :term:`global parameters` - (see the function documentation for details). - - 2. When sampling a scene, before sampling any other distributions the - :obj:`~ExternalSampler.sample` method of the `ExternalSampler` is - called to sample all the external parameters. For active samplers, this - method passes along the ``feedback`` value given to `Scenario.generate`, - if any. - - 3. Once the external parameters have values, the program is equivalent to - one without external parameters, and sampling proceeds as usual. As for - every instance of `Distribution`, the external parameters will have - their :obj:`~Samplable.sampleGiven` method called once all their - dependencies have been sampled; by default this method just returns the - value sampled for this parameter in step (2). - -.. note:: - - Note that while external parameters, like all instances of `Distribution`, - are allowed to have dependencies, they are an exception to the usual rule - that dependencies are always sampled before dependents, because the - `ExternalSampler.sample` method is called before any other sampling. - However, as explained above, the :obj:`~Samplable.sampleGiven` method is - called in the proper order and external samplers which need to do sampling - based on the values of other distributions can be invoked from it. The - two-step mechanism with `ExternalSampler.sample` is provided for samplers - which sample the whole space of external parameters at once (e.g. the - VerifAI samplers). - -Samplers from VerifAI -===================== - -The external sampling mechanism is designed to be extensible. The only built-in -`ExternalSampler` is the `VerifaiSampler`, which provides access to the -samplers in the `VerifAI`_ toolkit (which in turn can use Scenic as a modeling -language). - -The `VerifaiSampler` supports several types of external parameters corresponding -to the primitive distributions: `VerifaiRange` and `VerifaiDiscreteRange` for -continuous and discrete intervals, and `VerifaiOptions` for discrete sets. -For example, suppose we write:: - - ego = new Object at (VerifaiRange(5, 15), 0) - -This is equivalent to the ordinary Scenic line :scenic:`ego = new Object at (Range(5, 15), 0)`, -except that the X coordinate of the ego is sampled by VerifAI within the range -(5, 15) instead of being uniformly distributed over it. By default the -`VerifaiSampler` uses VerifAI's `Halton`_ sampler, so the range will still be -covered uniformly but more systematically. If we want to use a different sampler, -we can set the ``verifaiSamplerType`` global parameter:: - - param verifaiSamplerType = 'ce' - ego = new Object at (VerifaiRange(5, 15), 0) - -Now the X coordinate will be sampled using VerifAI's `cross-entropy`_ sampler. -If we pass a feedback value to `Scenario.generate` which scores the previous -scene, then the coordinate will not be sampled uniformly but rather converge to -a distribution concentrated on values minimizing the score. Active samplers like -cross-entropy can be used for falsification in this way, driving a system toward -parts of the parameter space where a specification is violated. - -The cross-entropy sampler in VerifAI can be started from a non-uniform prior. -Scenic provides a convenient way to define this prior using the ordinary syntax -for distributions:: - - param verifaiSamplerType = 'ce' - ego = new Object at (VerifaiParameter.withPrior(Normal(10, 3)), 0) - -Now cross-entropy sampling will start from a normal distribution with mean 10 -and standard deviation 3. Priors are restricted to primitive distributions and -in general may be approximated so that VerifAI can handle them -- see -`VerifaiParameter.withPrior` for details. - -To set a time bound when using VerifAI's dynamic sampling, set the ``timeBound`` -global parameter to value representing the upper bound on the number of timesteps -the sampler should account for. For example:: - - param timeBound = 250 - -This value can also be set directly in VerifAI via the ``maxSteps`` parameter to the -``ScenicSampler``. - -For more information on how to customize the sampler, see `VerifaiSampler`. - -.. _VerifAI: https://github.com/BerkeleyLearnVerify/VerifAI - -.. _Halton: https://en.wikipedia.org/wiki/Halton_sequence - -.. _cross-entropy: https://en.wikipedia.org/wiki/Cross-entropy_method - -""" - -from abc import ABC, abstractmethod -from importlib import metadata -import warnings - -from dotmap import DotMap -import numpy - -from scenic.core.distributions import Distribution, Options -from scenic.core.errors import InvalidScenarioError - - -class ExternalSampler: - """Abstract class for objects called to sample values for each external parameter. - - The initializer for this class takes the same arguments as the factory function - `forParameters` below. - - Attributes: - rejectionFeedback: Value passed to the `sample` method when the last sample was rejected. - This value can be chosen by a Scenic scenario using the global parameter - ``externalSamplerRejectionFeedback``. - """ - - def __init__(self, params, globalParams): - # feedback value passed to external sampler when the last scene was rejected - self.rejectionFeedback = globalParams.get("externalSamplerRejectionFeedback") - - @staticmethod - def forParameters(params, globalParams): - """Create an `ExternalSampler` given the sets of external and global parameters. - - The scenario may explicitly select an external sampler by assigning the - :term:`global parameter` ``externalSampler`` to a subclass of `ExternalSampler`. - Otherwise, a `VerifaiSampler` is used by default. - - Args: - params (tuple): Tuple listing each `ExternalParameter`. - globalParams (dict): Dictionary of global parameters for the `Scenario`, made - available here to support sampler customization through setting parameters. - Note that the values of these parameters may be instances of `Distribution`! - - Returns: - An `ExternalSampler` configured for the given parameters. - """ - if len(params) > 0: - externalSampler = globalParams.get("externalSampler", VerifaiSampler) - if not issubclass(externalSampler, ExternalSampler): - raise InvalidScenarioError( - f"externalSampler type {externalSampler}" - " not subclass of ExternalSampler" - ) - return externalSampler(params, globalParams) - else: - return None - - def sample(self, feedback): - """Sample values for all the external parameters. - - Args: - feedback: Feedback from the last sample (for active samplers). - """ - self.cachedSample = self.nextSample(feedback) - - def nextSample(self, feedback): - """Actually do the sampling. Implemented by subclasses.""" - raise NotImplementedError - - def valueFor(self, param): - """Return the sampled value for a parameter. Implemented by subclasses.""" - raise NotImplementedError - - -class VerifaiSampler(ExternalSampler): - """An external sampler exposing the samplers in the VerifAI toolkit. - - The sampler can be configured using the following Scenic :term:`global parameters`: - - * ``verifaiSamplerType`` -- sampler type (see the ``verifai.server.choose_sampler`` - function); the default is ``'halton'`` - * ``verifaiSamplerParams`` -- ``DotMap`` of options passed to the sampler - - The `VerifaiSampler` supports external parameters which are instances of `VerifaiParameter`. - """ - - def __init__(self, params, globalParams): - super().__init__(params, globalParams) - import verifai.features - import verifai.server - - self._verifaiDynamic = int(metadata.version("verifai").split(".")[0]) > 2 - - # construct FeatureSpace - timeBound = globalParams.get("timeBound", 0) - usingProbs = False - self.params = tuple(params) - for index, param in enumerate(self.params): - if not isinstance(param, VerifaiParameter): - raise RuntimeError( - f"VerifaiSampler given parameter of wrong type: {param}" - ) - param.sampler = self - param.index = index - if param.probs is not None: - usingProbs = True - - if not self._verifaiDynamic and any(param.isTimeSeries for param in self.params): - raise RuntimeError("TimeSeries not supported for VerifAI versions < 3.0") - - if timeBound == 0 and any(param.isTimeSeries for param in self.params): - warnings.warn( - "TimeSeries external parameter used but no global parameter `timeBound` is specified. " - "(If using VerifAI’s ScenicSampler, set its maxSteps option)." - ) - - fs_kwargs = {} - if self._verifaiDynamic: - fs_kwargs["timeBound"] = timeBound - - space = verifai.features.FeatureSpace( - { - self.nameForParam(index): ( - verifai.features.Feature(param.domain) - if not param.isTimeSeries - else verifai.features.TimeSeriesFeature(param.domain) - ) - for index, param in enumerate(self.params) - }, - **fs_kwargs, - ) - - # set up VerifAI sampler - samplerType = globalParams.get("verifaiSamplerType", "halton") - samplerParams = globalParams.get("verifaiSamplerParams", None) - if usingProbs and samplerType == "ce": - if samplerParams is None: - samplerParams = DotMap() - else: - samplerParams = samplerParams.copy() # avoid mutating original - if "cont" in samplerParams or "disc" in samplerParams: - raise RuntimeError( - "CE distributions specified in both VerifaiParameters" - " and verifaiSamplerParams" - ) - cont_buckets = [] - cont_dists = [] - disc_dists = [] - for param in self.params: - if isinstance(param, VerifaiRange): - if param.probs is None: - buckets = 5 - dist = numpy.ones(buckets) / buckets - else: - dist = numpy.array(param.probs) - buckets = len(dist) - cont_buckets.append(buckets) - cont_dists.append(dist) - elif isinstance(param, VerifaiDiscreteRange): - n = param.high - param.low + 1 - dist = ( - numpy.ones(n) / n - if param.probs is None - else numpy.array(param.probs) - ) - disc_dists.append(dist) - else: - raise RuntimeError(f"Parameter {param} not supported by CE sampler") - samplerParams.cont.buckets = cont_buckets - samplerParams.cont.dist = numpy.array(cont_dists) - samplerParams.disc.dist = numpy.array(disc_dists) - data = verifai.server.choose_sampler( - space, samplerType, sampler_params=samplerParams - ) - if not data: - raise RuntimeError(f'Unknown VerifAI sampler type "{samplerType}"') - self.sampler = data[1] - - # default rejection feedback is positive so cross-entropy sampler won't update; - # for other active samplers an appropriate value should be set manually - if self.rejectionFeedback is None: - self.rejectionFeedback = 1 - self.cachedSample = None - - self._lastSample = None - self._lastInfo = None - self._lastDynamicSample = None - self._lastSimulation = None - self._lastTime = -1 - - def nextSample(self, feedback): - if feedback is not None: - assert self._lastSample is not None - if self._verifaiDynamic: - self._lastSample.complete(feedback) - else: - self.sampler.update(self._lastSample, self._lastInfo, feedback) - - if self._verifaiDynamic: - self._lastSample = self.sampler.getSample() - else: - lastSample = self.sampler.getSample() - self._lastSample = lastSample[0] - self._lastInfo = lastSample[1] - return self._lastSample - - def nextDynamicSample(self): - import scenic.syntax.veneer as veneer - - assert veneer.currentSimulation is not None - - if self._lastSimulation is not veneer.currentSimulation: - self._lastSimulation = veneer.currentSimulation - self._lastTime = -1 - - if veneer.currentSimulation.currentTime > self._lastTime: - feedback = veneer.currentSimulation - self._lastDynamicSample = self.cachedSample.getDynamicSample(feedback) - self._lastTime = veneer.currentSimulation.currentTime - - return self._lastDynamicSample - - def valueFor(self, param): - if not param.isTimeSeries: - if self._verifaiDynamic: - sampleTarget = self.cachedSample.staticSample - else: - sampleTarget = self.cachedSample - return param.extractOutput( - getattr(sampleTarget, self.nameForParam(param.index)) - ) - else: - callback = lambda: param.extractOutput( - getattr( - self.nextDynamicSample(), - self.nameForParam(param.index), - ) - ) - return TimeSeriesParameter(callback) - - @staticmethod - def nameForParam(i): - """Parameter name for a given index in the Feature Space.""" - return f"param{i}" - - -class ExternalParameter(Distribution): - """A value determined by external code rather than Scenic's internal sampler.""" - - def __init__(self): - super().__init__() - self.sampler = None - self.isTimeSeries = False - import scenic.syntax.veneer as veneer # TODO improve? - - veneer.registerExternalParameter(self) - - def sampleGiven(self, value): - """Specialization of `Samplable.sampleGiven` for external parameters. - - By default, this method simply looks up the value previously sampled by - `ExternalSampler.sample`. - """ - assert self.sampler is not None - return self.sampler.valueFor(self) - - def extractOutput(self, value): - """ - Given a raw sampled value for a parameter, optionally extract the actual desired value. - - By default just passes the value through unchanged. - """ - return value - - -class TimeSeriesParameter: - def __init__(self, callback): - self._callback = callback - self._lastSimulation = None - self._lastTime = -1 - - def getSample(self): - import scenic.syntax.veneer as veneer - - assert veneer.currentSimulation is not None - - if self._lastSimulation is not veneer.currentSimulation: - self._lastSimulation = veneer.currentSimulation - self._lastTime = -1 - - if veneer.currentSimulation.currentTime <= self._lastTime: - raise RuntimeError( - "Attempted `getSample` for a TimeSeries external parameter twice in one timestep." - ) - - self._lastTime = veneer.currentSimulation.currentTime - return self._callback() - - -def TimeSeries(param): - if not isinstance(param, ExternalParameter): - raise TypeError("Cannot turn a non `ExternalParameter` into a time series") - - param.isTimeSeries = True - return param - - -class VerifaiParameter(ExternalParameter): - """An external parameter sampled using one of VerifAI's samplers.""" - - def __init__(self, domain): - super().__init__() - self.domain = domain - - @staticmethod - def withPrior(dist, buckets=None): - """Creates a `VerifaiParameter` using the given distribution as a prior. - - Since the VerifAI cross-entropy sampler currently only supports piecewise-constant - distributions, if the prior is not of that form it may be approximated. For most - built-in distributions, the approximation is exact: for a particular distribution, - check its `bucket` method. - """ - if not dist.isPrimitive: - raise RuntimeError( - "VerifaiParameter.withPrior called on " - f"non-primitive distribution {dist}" - ) - bucketed = dist.bucket(buckets=buckets) - return VerifaiOptions( - bucketed.optWeights if bucketed.optWeights else bucketed.options - ) - - -class VerifaiRange(VerifaiParameter): - """A :obj:`~scenic.core.distributions.Range` (real interval) sampled by VerifAI.""" - - _defaultValueType = float - - def __init__(self, low, high, buckets=None, weights=None): - import verifai.features - - super().__init__(verifai.features.Box([low, high])) - if weights is not None: - weights = tuple(weights) - if buckets is not None and len(weights) != buckets: - raise RuntimeError( - f"VerifaiRange created with {len(weights)} weights " - f"but {buckets} buckets" - ) - elif buckets is not None: - weights = [1] * buckets - else: - self.probs = None - return - total = sum(weights) - self.probs = tuple(wt / total for wt in weights) - - def extractOutput(self, value): - assert len(value) == 1 - return value[0] - - -class VerifaiDiscreteRange(VerifaiParameter): - """A :obj:`~scenic.core.distributions.DiscreteRange` (integer interval) sampled by VerifAI.""" - - _defaultValueType = float - - def __init__(self, low, high, weights=None): - import verifai.features - - super().__init__(verifai.features.DiscreteBox([low, high])) - if weights is not None: - if len(weights) != (high - low + 1): - raise RuntimeError( - f"VerifaiDiscreteRange created with {len(weights)} weights " - f"for {high - low + 1} values" - ) - total = sum(weights) - self.probs = tuple(wt / total for wt in weights) - else: - self.probs = None - - def extractOutput(self, value): - assert len(value) == 1 - return value[0] - - -class VerifaiOptions(Options): - """An :obj:`~scenic.core.distributions.Options` (discrete set) sampled by VerifAI.""" - - @staticmethod - def makeSelector(n, weights): - return VerifaiDiscreteRange(0, n, weights) diff --git a/src/scenic/core/geometry.py b/src/scenic/core/geometry.py index 343f08477..b28bcf24a 100644 --- a/src/scenic/core/geometry.py +++ b/src/scenic/core/geometry.py @@ -29,6 +29,11 @@ def cos(x) -> float: return math.cos(x) +@distributionFunction +def tan(x) -> float: + return math.tan(x) + + @monotonicDistributionFunction def hypot(*args) -> float: return math.hypot(*args) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 90184af04..7e43fb55b 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -8,6 +8,7 @@ """ from abc import ABC, abstractmethod +import bisect import itertools import math import random @@ -79,6 +80,11 @@ ################################################################################################### +# TODO: Move this somewhere else? +def randomIndexFromVal(val, weights): + return bisect.bisect(weights, val * weights[-1]) + + class Region(Samplable, ABC): """An abstract base class for Scenic Regions""" @@ -93,6 +99,14 @@ def uniformPointInner(self): """Do the actual random sampling. Implemented by subclasses.""" pass + @abstractmethod + def parameterizedUniformPointInner(self, vals): + """Sample from this region deterministically, given vals. Implemented by subclasses. + + vals contains at least 3 distributions in [0,1]. + """ + pass + @abstractmethod def containsPoint(self, point) -> bool: """Check if the `Region` contains a point. Implemented by subclasses.""" @@ -141,6 +155,11 @@ def dimensionality(self): def size(self): return None + @property + @abstractmethod + def _sampleVals(self): + pass + def intersects(self, other, triedReversed=False) -> bool: """intersects(other) @@ -276,13 +295,19 @@ def midpoint(self): class PointInRegionDistribution(VectorDistribution): """Uniform distribution over points in a Region""" - def __init__(self, region, tag=None): - super().__init__(region) + def __init__(self, region, tag=None, sampleVals=None): + super().__init__(region, sampleVals) self.region = region self.tag = tag + self.sampleVals = sampleVals def sampleGiven(self, value): - return value[self.region].uniformPointInner() + if self.sampleVals is None: + return value[self.region].uniformPointInner() + else: + return value[self.region].parameterizedUniformPointInner( + value[self.sampleVals] + ) @property def heading(self): @@ -304,6 +329,10 @@ def z(self) -> float: def __repr__(self): return f"PointIn({self.region!r})" + @property + def _deterministic(self): + return self.sampleVals is not None + ################################################################################################### # Utility Regions and Functions @@ -325,6 +354,9 @@ def union(self, other, triedReversed=False): def uniformPointInner(self): raise RuntimeError(f"Attempted to sample from everywhere (AllRegion)") + def parameterizedUniformPointInner(self, vals): + raise RuntimeError(f"Attempted to sample from everywhere (AllRegion)") + def containsPoint(self, point): return True @@ -347,6 +379,10 @@ def closestPointTo(self, target): def AABB(self): raise TypeError("AllRegion does not have a well defined AABB") + @property + def _sampleVals(self): + return (0,) + @property def dimensionality(self): return float("inf") @@ -380,6 +416,9 @@ def union(self, other, triedReversed=False): def uniformPointInner(self): raise RejectionException(f"sampling empty Region") + def parameterizedUniformPointInner(self, vals): + raise RejectionException(f"sampling empty Region") + def containsPoint(self, point): return False @@ -402,6 +441,10 @@ def closestPointTo(self, target): def AABB(self): raise TypeError("EmptyRegion does not have a well defined AABB") + @property + def _sampleVals(self): + return (0,) + @property def dimensionality(self): return 0 @@ -492,6 +535,11 @@ def closestPointTo(self, target): def AABB(self): raise NotImplementedError + @property + def _sampleVals(self): + raise NotImplementedError + return tuple(r._sampleVals for r in self.regions) + @cached_property def footprint(self): return convertToFootprint(self) @@ -502,8 +550,12 @@ def uniformPointInner(self): sampler = self.genericSampler return self.orient(sampler(self)) + def parameterizedUniformPointInner(self, vals): + assert len(vals) == len(self.regions) + return self.orient(self.genericSampler(self, vals)) + @staticmethod - def genericSampler(intersection): + def genericSampler(intersection, vals=None): regs = intersection.regions # Filter out all regions with known dimensionality greater than the minimum known_dim_regions = [ @@ -519,7 +571,10 @@ def genericSampler(intersection): for reg in sampling_regions: try: - point = reg.uniformPointInner() + if vals is None: + point = reg.uniformPointInner() + else: + point = reg.parameterizedUniformPointInner(vals[regs.index(reg)]) except UndefinedSamplingException: num_regs_undefined += 1 continue @@ -611,6 +666,11 @@ def closestPointTo(self, target): def AABB(self): raise NotImplementedError + @property + def _sampleVals(self): + raise NotImplementedError + return (1,) + tuple(r._sampleVals for r in self.regions) + @cached_property def footprint(self): return convertToFootprint(self) @@ -621,8 +681,12 @@ def uniformPointInner(self): sampler = self.genericSampler return self.orient(sampler(self)) + def parameterizedUniformPointInner(self, vals): + assert len(vals) == 1 + len(self.regions) + return self.orient(self.genericSampler(self, vals)) + @staticmethod - def genericSampler(union): + def genericSampler(union, vals=None): regs = union.regions # Check that all regions have well defined dimensionality @@ -638,13 +702,21 @@ def genericSampler(union): # Check that all large regions have well defined size if any(reg.size is None or reg.size == float("inf") for reg in large_regs): raise UndefinedSamplingException( - f"cannot sample union of Regions {regs} with " "ill-defined size" + f"cannot sample union of Regions {regs} with ill-defined size" ) # Pick a sample, weighted by region size reg_sizes = tuple(reg.size for reg in large_regs) - target_reg = random.choices(large_regs, weights=reg_sizes)[0] - point = target_reg.uniformPointInner() + + if vals is None: + target_reg = random.choices(large_regs, weights=reg_sizes)[0] + point = target_reg.uniformPointInner() + else: + target_values = vals[1 + regs.index(target_reg)] + cum_sizes = list(itertools.accumulate(reg_sizes)) + target_reg = large_regs[randomIndexFromVal(vals[0], cum_sizes)] + + point = target_reg.parameterizedUniformPointInner(target_values) # Potentially reject based on containment of the sample containment_count = sum(int(reg._trueContainsPoint(point)) for reg in regs) @@ -715,6 +787,11 @@ def closestPointTo(self, target): def AABB(self): raise NotImplementedError + @property + def _sampleVals(self): + raise NotImplementedError + return self.regionA._sampleVals + @cached_property def footprint(self): return convertToFootprint(self) @@ -725,10 +802,16 @@ def uniformPointInner(self): sampler = self.genericSampler return self.orient(sampler(self)) + def parameterizedUniformPointInner(self): + return self.orient(self.genericSampler(self)) + @staticmethod - def genericSampler(difference): + def genericSampler(difference, vals=None): regionA, regionB = difference.regionA, difference.regionB - point = regionA.uniformPointInner() + if vals is None: + point = regionA.uniformPointInner() + else: + point = regionA.parameterizedUniformPointInner(vals) if regionB._trueContainsPoint(point): raise RejectionException( f"sampling difference of Regions {regionA} and {regionB}" @@ -1781,6 +1864,16 @@ def uniformPointInner(self): else: return Vector(*sample[0]) + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 3 + point = Vector( + *(numpy.asarray(vals[:3]) * self.mesh.extents + self.mesh.bounds[0]) + ) + if self.containsPoint(point): + return point + else: + raise RejectionException + @distributionFunction def distanceTo(self, point): """Get the minimum distance from this region to the specified point.""" @@ -1842,6 +1935,10 @@ def isConvex(self): def dimensionality(self): return 3 + @property + def _sampleVals(self): + return (3,) + @cached_property def size(self): return self.mesh.mass / self.mesh.density @@ -2137,6 +2234,9 @@ def containsRegionInner(self, reg, tolerance): def uniformPointInner(self): return Vector(*trimesh.sample.sample_surface(self.mesh, 1)[0][0]) + def parameterizedUniformPointInner(self, vals): + raise NotImplementedError # TODO + @distributionFunction def distanceTo(self, point): """Get the minimum distance from this object to the specified point.""" @@ -2152,6 +2252,10 @@ def distanceTo(self, point): def dimensionality(self): return 2 + @property + def _sampleVals(self): + return (3,) + @cached_property def size(self): return self.mesh.area @@ -2366,6 +2470,9 @@ def uniformPointInner(self): return Vector(*offset_pt) + def parameterizedUniformPointInner(self, vals): + raise NotImplementedError() + def dilation(self, iterations, structure=None): """Returns a dilated/eroded version of this VoxelRegion. @@ -2493,6 +2600,10 @@ def AABB(self): tuple(self.voxelGrid.bounds[1]), ) + @property + def _sampleVals(self): + return (3,) + @property def size(self): return self.voxelGrid.volume @@ -2591,6 +2702,11 @@ def uniformPointInner(self): f"Attempted to sample from a PolygonalFootprintRegion, for which uniform sampling is undefined" ) + def parameterizedUniformPointInner(self): + raise UndefinedSamplingException( + f"Attempted to sample from a PolygonalFootprintRegion, for which uniform sampling is undefined" + ) + def containsPoint(self, point): """Checks if a point is contained in the polygonal footprint. @@ -2625,10 +2741,10 @@ def containsObject(self, obj): def containsRegionInner(self, reg, tolerance): buffered_polygons = self.polygons.buffer(tolerance) - if isinstance(other, MeshRegion): + if isinstance(reg, MeshRegion): return buffered_polygons.contains(reg._boundingPolygon) - if isinstance(other, (PolygonalRegion, PolygonalFootprintRegion)): + if isinstance(reg, (PolygonalRegion, PolygonalFootprintRegion)): return buffered_polygons.contains(reg.polygons) raise NotImplementedError @@ -2658,6 +2774,10 @@ def AABB(self): def dimensionality(self): return 3 + @property + def _sampleVals(self): + return (0,) + @property def size(self): return float("inf") @@ -2831,6 +2951,8 @@ def __init__( self.edge_lengths.append(c1.distanceTo(c2)) + self.cum_edge_lengths = tuple(itertools.accumulate(self.edge_lengths)) + self.tolerance = tolerance self._edgeVectorArray = numpy.asarray( @@ -2899,14 +3021,30 @@ def AABB(self): tuple(numpy.amax(self.vertices, axis=0)), ) + @property + def _sampleVals(self): + return (1,) + def uniformPointInner(self): - # Pick an edge, weighted by length, and extract its two points - edge = random.choices(population=self.edges, weights=self.edge_lengths, k=1)[0] - v1, v2 = edge - c1, c2 = self.vert_to_vec[v1], self.vert_to_vec[v2] + return self.parameterizedUniformPointInner([random.random()]) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 1 + + # Pick a random length along the path, and sample the point at that distance along the path. + length_along = vals[0] * self.cum_edge_lengths[-1] + edge_i = randomIndexFromVal(vals[0], self.cum_edge_lengths) + edge_dist = ( + length_along + if edge_i == 0 + else length_along - self.cum_edge_lengths[edge_i - 1] + ) + edge_fraction = edge_dist / self.edge_lengths[edge_i] + v1, v2 = self.edges[edge_i] + c1, c2 = self.vert_to_vec[v1], self.vert_to_vec[v2] # Sample uniformly from the line segment - sampled_pt = c1 + random.uniform(0, 1) * (c2 - c1) + sampled_pt = c1 + edge_fraction * (c2 - c1) return sampled_pt @@ -2916,7 +3054,7 @@ def dimensionality(self): @cached_property def size(self): - return sum(self.edge_lengths) + return self.cum_edge_lengths[-1] ################################################################################################### @@ -3052,6 +3190,19 @@ def uniformPointInner(self): if shapely.intersects_xy(triangle, x, y): return self.orient(Vector(x, y, self.z)) + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 3 + + trisAndBounds, cumulativeAreas = self._samplingData + triangle, bounds = trisAndBounds[randomIndexFromVal(vals[0], cumulativeAreas)] + minx, miny, maxx, maxy = bounds + + x, y = minx + vals[1] * (maxx - minx), miny + vals[2] * (maxy - miny) + if shapely.intersects_xy(triangle, x, y): + return self.orient(Vector(x, y, self.z)) + else: + raise RejectionException + @distributionFunction def intersects(self, other, triedReversed=False): if isinstance(other, PolygonalRegion): @@ -3217,6 +3368,10 @@ def AABB(self): xmin, ymin, xmax, ymax = self.polygons.bounds return ((xmin, ymin, self.z), (xmax, ymax, self.z)) + @property + def _sampleVals(self): + return (3,) + @distributionFunction def buffer(self, amount): buffered_polygons = self.polygons.buffer(amount) @@ -3703,15 +3858,17 @@ def defaultOrientation(self, point): return start.angleTo(end) def uniformPointInner(self): - pointA, pointB = random.choices( - self.segments, cum_weights=self.cumulativeLengths - )[0] - interpolation = random.random() - x, y = averageVectors(pointA, pointB, weight=interpolation) + return self.parameterizedUniformPointInner([random.random()]) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 1 + + pt = self.pointAlongBy(vals[0], normalized=True) + if self._usingDefaultOrientation: - return OrientedVector(x, y, 0, headingOfSegment(pointA, pointB)) + return OrientedVector(pt.x, pt.y, 0, self.defaultOrientation(pt)) else: - return self.orient(Vector(x, y, 0)) + return self.orient(pt) def containsRegionInner(self, other, tolerance): poly = toPolygon(other) @@ -3865,6 +4022,10 @@ def AABB(self): xmin, ymin, xmax, ymax = self.lineString.bounds return ((xmin, ymin, 0), (xmax, ymax, 0)) + @property + def _sampleVals(self): + return (1,) + def show(self, plt, style="r-", **kwargs): plotPolygon(self.lineString, plt, style=style, **kwargs) @@ -3960,7 +4121,11 @@ def __init__(self, name, points, kdTree=None, orientation=None, tolerance=1e-6): self.tolerance = tolerance def uniformPointInner(self): - i = random.randrange(0, len(self.points)) + return self.parameterizedUniformPointInner([random.random()]) + + def parameterizedUniformPointInner(self, vals): + # assert len(vals) == 1 + i = int(vals[0] * len(self.points)) return self.orient(Vector(*self.points[i])) def intersects(self, other, triedReversed=False): @@ -4032,6 +4197,10 @@ def AABB(self): tuple(numpy.amax(self.points, axis=0)), ) + @property + def _sampleVals(self): + return (1,) + def __eq__(self, other): if type(other) is not PointSetRegion: return NotImplemented diff --git a/src/scenic/core/scenarios.py b/src/scenic/core/scenarios.py index 2476a4653..984fd108e 100644 --- a/src/scenic/core/scenarios.py +++ b/src/scenic/core/scenarios.py @@ -289,9 +289,7 @@ def __init__( self.egoObject = egoObject self.params = dict(params) self.externalParams = tuple(externalParams) - self.externalSampler = ExternalSampler.forParameters( - self.externalParams, self.params - ) + self.externalSampler = None self.monitors = tuple(monitors) self.behaviorNamespaces = behaviorNamespaces self.dynamicScenario = dynamicScenario @@ -329,6 +327,15 @@ def setSampleChecker(self, checker): self.checker = checker self.checker.setRequirements(self.defaultRequirements + self.userRequirements) + def createExternalSampler(self, externalParams): + assert self.externalSampler is None + self.externalParams += tuple( + p for p in set(externalParams) if p not in self.externalParams + ) + self.externalSampler = ExternalSampler.forParameters( + self.externalParams, self.params + ) + def containerOfObject(self, obj): if hasattr(obj, "regionContainedIn") and obj.regionContainedIn is not None: return obj.regionContainedIn diff --git a/src/scenic/core/vectors.py b/src/scenic/core/vectors.py index 4b399df72..a482babcb 100644 --- a/src/scenic/core/vectors.py +++ b/src/scenic/core/vectors.py @@ -56,6 +56,8 @@ def toVector(self): class VectorOperatorDistribution(VectorDistribution): + _deterministic = True + """Vector version of OperatorDistribution.""" def __init__(self, operator, obj, operands): diff --git a/src/scenic/core/workspaces.py b/src/scenic/core/workspaces.py index f3be2b943..63ffc4fa0 100644 --- a/src/scenic/core/workspaces.py +++ b/src/scenic/core/workspaces.py @@ -92,6 +92,9 @@ def scenicToSchematicCoords(self, coords): def uniformPointInner(self): return self.region.uniformPointInner() + def parameterizedUniformPointInner(self): + return self.region.parameterizedUniformPointInner() + def intersect(self, other, triedReversed=False): return self.region.intersect(other, triedReversed) @@ -126,6 +129,10 @@ def closestPointTo(self, target): def AABB(self): return self.region.AABB + @property + def _sampleVals(self): + return self.region._sampleVals + @property def dimensionality(self): return self.region.dimensionality diff --git a/src/scenic/simulators/utils/colors.py b/src/scenic/simulators/utils/colors.py index 7f3a0bed9..1bfe8b747 100644 --- a/src/scenic/simulators/utils/colors.py +++ b/src/scenic/simulators/utils/colors.py @@ -78,6 +78,8 @@ class NoisyColorDistribution(Distribution): lightNoise (float): noise to add to base lightness """ + _deterministic = True + def __init__(self, baseColor, hueNoise, satNoise, lightNoise): super().__init__(baseColor, hueNoise, satNoise, lightNoise, valueType=Color) self.baseColor = baseColor diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index cbb7ab877..d733d2bd3 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -29,16 +29,19 @@ import importlib.util import inspect import io +import itertools import os import sys import time import types from typing import Optional +import warnings -from scenic.core.distributions import RejectionException, toDistribution +from scenic.core.distributions import Distribution, RejectionException, toDistribution from scenic.core.dynamics.scenarios import DynamicScenario import scenic.core.errors as errors from scenic.core.errors import InvalidScenarioError, PythonCompileError +from scenic.core.external_params import ExternalParameter, ExternalSampler from scenic.core.lazy_eval import needsLazyEvaluation import scenic.core.pruning as pruning from scenic.core.serialization import deterministicHash @@ -703,4 +706,40 @@ def isModularScenario(thing): # Validate scenario scenario.validate() + # Convert distributions to ExternalParameters if requested, and create + # the external sampler. + if scenario.params.get("convertDistributions", False): + from scenic.core.external_params.verifai import VerifaiSampler + + externalParamConverter = scenario.params.get( + "externalSampler", VerifaiSampler + ).getExternalParameterConverter() + + for dep in scenario.dependencies: + externalParamConverter.convert(dep) + print(f"{dep}: {len(set(externalParamConverter.externalParams))}") + + ## DEBUG ## + test = { + d + for dep in scenario.dependencies + for d in dep.recursiveDependencies() + if ( + isinstance(d, Distribution) + and not d._conditioned._deterministic + and not isinstance(d._conditioned, ExternalParameter) + ) + } + for d in test: + print() + print(type(d)) + print(d) + print() + + newExternalParams = externalParamConverter.externalParams + else: + newExternalParams = [] + + scenario.createExternalSampler(newExternalParams) + return scenario diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index c81975e06..ed9b3affe 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -34,6 +34,7 @@ "record_final", "sin", "cos", + "tan", "hypot", "max", "min", @@ -127,6 +128,12 @@ "VerifaiRange", "VerifaiDiscreteRange", "VerifaiOptions", + "VerifaiSampler", + "OptunaRange", + "OptunaDiscreteRange", + "OptunaParameter", + "OptunaOptions", + "OptunaSampler", "TimeSeries", "File", "Files", @@ -208,14 +215,22 @@ ) from scenic.core.dynamics.invocables import BlockConclusion, runTryInterrupt from scenic.core.dynamics.scenarios import DynamicScenario -from scenic.core.external_params import ( - TimeSeries, +from scenic.core.external_params import TimeSeries +from scenic.core.external_params.optuna import ( + OptunaDiscreteRange, + OptunaOptions, + OptunaParameter, + OptunaRange, + OptunaSampler, +) +from scenic.core.external_params.verifai import ( VerifaiDiscreteRange, VerifaiOptions, VerifaiParameter, VerifaiRange, + VerifaiSampler, ) -from scenic.core.geometry import cos, hypot, max, min, sin +from scenic.core.geometry import cos, hypot, max, min, sin, tan from scenic.core.object_types import Mutator, Object, OrientedPoint, Point from scenic.core.regions import ( BoxRegion, From 783549e7bee34e29f01744a2459fbc51713172ff Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 13:05:32 -0700 Subject: [PATCH 117/134] Fix import issue? --- src/scenic/syntax/translator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index d733d2bd3..2f2285e66 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -41,7 +41,6 @@ from scenic.core.dynamics.scenarios import DynamicScenario import scenic.core.errors as errors from scenic.core.errors import InvalidScenarioError, PythonCompileError -from scenic.core.external_params import ExternalParameter, ExternalSampler from scenic.core.lazy_eval import needsLazyEvaluation import scenic.core.pruning as pruning from scenic.core.serialization import deterministicHash @@ -709,6 +708,7 @@ def isModularScenario(thing): # Convert distributions to ExternalParameters if requested, and create # the external sampler. if scenario.params.get("convertDistributions", False): + from scenic.core.external_params import ExternalParameter from scenic.core.external_params.verifai import VerifaiSampler externalParamConverter = scenario.params.get( From 1acda627c4ab656004722f4f14a02ade34d4297f Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 13:06:19 -0700 Subject: [PATCH 118/134] Another import issue? --- src/scenic/syntax/veneer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index ed9b3affe..357a70fb2 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -215,7 +215,7 @@ ) from scenic.core.dynamics.invocables import BlockConclusion, runTryInterrupt from scenic.core.dynamics.scenarios import DynamicScenario -from scenic.core.external_params import TimeSeries +from scenic.core.external_params.external_params import TimeSeries from scenic.core.external_params.optuna import ( OptunaDiscreteRange, OptunaOptions, From ac2fd55c909c86d1dcef5f6425c6667491cf3b77 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 13:08:19 -0700 Subject: [PATCH 119/134] Added missing module. --- src/scenic/core/external_params/__init__.py | 3 + .../core/external_params/external_params.py | 285 ++++++++++++++++++ src/scenic/core/external_params/optuna.py | 198 ++++++++++++ src/scenic/core/external_params/verifai.py | 263 ++++++++++++++++ src/scenic/syntax/translator.py | 2 +- src/scenic/syntax/veneer.py | 2 +- 6 files changed, 751 insertions(+), 2 deletions(-) create mode 100644 src/scenic/core/external_params/__init__.py create mode 100644 src/scenic/core/external_params/external_params.py create mode 100644 src/scenic/core/external_params/optuna.py create mode 100644 src/scenic/core/external_params/verifai.py diff --git a/src/scenic/core/external_params/__init__.py b/src/scenic/core/external_params/__init__.py new file mode 100644 index 000000000..0118345f3 --- /dev/null +++ b/src/scenic/core/external_params/__init__.py @@ -0,0 +1,3 @@ +"""Scenic code related to external samplers.""" + +from scenic.core.external_params.external_params import * diff --git a/src/scenic/core/external_params/external_params.py b/src/scenic/core/external_params/external_params.py new file mode 100644 index 000000000..d9ebf4383 --- /dev/null +++ b/src/scenic/core/external_params/external_params.py @@ -0,0 +1,285 @@ +"""Support for values which are sampled outside of Scenic. + +External Samplers in General +============================ + +External samplers provide a mechanism to use different types of sampling +techniques, like optimization or quasi-random sampling, from within a Scenic +program. Ordinary random values in Scenic are instances of `Distribution`; +this module defines a special subclass, `ExternalParameter`, representing a +value which is sampled externally. Scenic programs with external parameters +are handled as follows: + + 1. During compilation, all instances of `ExternalParameter` are gathered + together and given to the `ExternalSampler.forParameters` function; + this function creates an appropriate `ExternalSampler`, + whose configuration can be controlled using :term:`global parameters` + (see the function documentation for details). + + 2. When sampling a scene, before sampling any other distributions the + :obj:`~ExternalSampler.sample` method of the `ExternalSampler` is + called to sample all the external parameters. For active samplers, this + method passes along the ``feedback`` value given to `Scenario.generate`, + if any. + + 3. Once the external parameters have values, the program is equivalent to + one without external parameters, and sampling proceeds as usual. As for + every instance of `Distribution`, the external parameters will have + their :obj:`~Samplable.sampleGiven` method called once all their + dependencies have been sampled; by default this method just returns the + value sampled for this parameter in step (2). + +.. note:: + + Note that while external parameters, like all instances of `Distribution`, + are allowed to have dependencies, they are an exception to the usual rule + that dependencies are always sampled before dependents, because the + `ExternalSampler.sample` method is called before any other sampling. + However, as explained above, the :obj:`~Samplable.sampleGiven` method is + called in the proper order and external samplers which need to do sampling + based on the values of other distributions can be invoked from it. The + two-step mechanism with `ExternalSampler.sample` is provided for samplers + which sample the whole space of external parameters at once (e.g. the + VerifAI samplers). + +Samplers from VerifAI +===================== + +The external sampling mechanism is designed to be extensible. The only built-in +`ExternalSampler` is the `VerifaiSampler`, which provides access to the +samplers in the `VerifAI`_ toolkit (which in turn can use Scenic as a modeling +language). + +The `VerifaiSampler` supports several types of external parameters corresponding +to the primitive distributions: `VerifaiRange` and `VerifaiDiscreteRange` for +continuous and discrete intervals, and `VerifaiOptions` for discrete sets. +For example, suppose we write:: + + ego = new Object at (VerifaiRange(5, 15), 0) + +This is equivalent to the ordinary Scenic line :scenic:`ego = new Object at (Range(5, 15), 0)`, +except that the X coordinate of the ego is sampled by VerifAI within the range +(5, 15) instead of being uniformly distributed over it. By default the +`VerifaiSampler` uses VerifAI's `Halton`_ sampler, so the range will still be +covered uniformly but more systematically. If we want to use a different sampler, +we can set the ``verifaiSamplerType`` global parameter:: + + param verifaiSamplerType = 'ce' + ego = new Object at (VerifaiRange(5, 15), 0) + +Now the X coordinate will be sampled using VerifAI's `cross-entropy`_ sampler. +If we pass a feedback value to `Scenario.generate` which scores the previous +scene, then the coordinate will not be sampled uniformly but rather converge to +a distribution concentrated on values minimizing the score. Active samplers like +cross-entropy can be used for falsification in this way, driving a system toward +parts of the parameter space where a specification is violated. + +The cross-entropy sampler in VerifAI can be started from a non-uniform prior. +Scenic provides a convenient way to define this prior using the ordinary syntax +for distributions:: + + param verifaiSamplerType = 'ce' + ego = new Object at (VerifaiParameter.withPrior(Normal(10, 3)), 0) + +Now cross-entropy sampling will start from a normal distribution with mean 10 +and standard deviation 3. Priors are restricted to primitive distributions and +in general may be approximated so that VerifAI can handle them -- see +`VerifaiParameter.withPrior` for details. + +To set a time bound when using VerifAI's dynamic sampling, set the ``timeBound`` +global parameter to value representing the upper bound on the number of timesteps +the sampler should account for. For example:: + + param timeBound = 250 + +This value can also be set directly in VerifAI via the ``maxSteps`` parameter to the +``ScenicSampler``. + +For more information on how to customize the sampler, see `VerifaiSampler`. + +.. _VerifAI: https://github.com/BerkeleyLearnVerify/VerifAI + +.. _Halton: https://en.wikipedia.org/wiki/Halton_sequence + +.. _cross-entropy: https://en.wikipedia.org/wiki/Cross-entropy_method + +""" + +from abc import ABC, abstractmethod +from importlib import metadata +from typing import Tuple +import warnings + +from dotmap import DotMap +import numpy + +from scenic.core.distributions import Distribution, Samplable +from scenic.core.errors import InvalidScenarioError + + +class ExternalSampler: + """Abstract class for objects called to sample values for each external parameter. + + The initializer for this class takes the same arguments as the factory function + `forParameters` below. + + Attributes: + rejectionFeedback: Value passed to the `sample` method when the last sample was rejected. + This value can be chosen by a Scenic scenario using the global parameter + ``externalSamplerRejectionFeedback``. + """ + + def __init__(self, params, globalParams): + # feedback value passed to external sampler when the last scene was rejected + self.rejectionFeedback = globalParams.get("externalSamplerRejectionFeedback") + + @classmethod + def getExternalParamType(cls): + return ExternalParameter + + @classmethod + def getExternalParameterConverter(cls): + return ExternalParameterConverter(cls, cls.getExternalParamType()) + + @staticmethod + def forParameters(params, globalParams): + """Create an `ExternalSampler` given the sets of external and global parameters. + + The scenario may explicitly select an external sampler by assigning the + :term:`global parameter` ``externalSampler`` to a subclass of `ExternalSampler`. + Otherwise, a `VerifaiSampler` is used by default. + + Args: + params (tuple): Tuple listing each `ExternalParameter`. + globalParams (dict): Dictionary of global parameters for the `Scenario`, made + available here to support sampler customization through setting parameters. + Note that the values of these parameters may be instances of `Distribution`! + + Returns: + An `ExternalSampler` configured for the given parameters. + """ + if len(params) > 0: + from scenic.core.external_params.verifai import VerifaiSampler + + externalSampler = globalParams.get("externalSampler", VerifaiSampler) + if not issubclass(externalSampler, ExternalSampler): + raise InvalidScenarioError( + f"externalSampler type {externalSampler}" + " not subclass of ExternalSampler" + ) + return externalSampler(params, globalParams) + else: + return None + + def sample(self, feedback): + """Sample values for all the external parameters. + + Args: + feedback: Feedback from the last sample (for active samplers). + """ + self.cachedSample = self.nextSample(feedback) + + def nextSample(self, feedback): + """Actually do the sampling. Implemented by subclasses.""" + raise NotImplementedError + + def valueFor(self, param): + """Return the sampled value for a parameter. Implemented by subclasses.""" + raise NotImplementedError + + +class ExternalParameter(Distribution): + """A value determined by external code rather than Scenic's internal sampler.""" + + def __init__(self): + super().__init__() + self.sampler = None + self.isTimeSeries = False + import scenic.syntax.veneer as veneer # TODO improve? + + veneer.registerExternalParameter(self) + + def sampleGiven(self, value): + """Specialization of `Samplable.sampleGiven` for external parameters. + + By default, this method simply looks up the value previously sampled by + `ExternalSampler.sample`. + """ + assert self.sampler is not None + return self.sampler.valueFor(self) + + def extractOutput(self, value): + """ + Given a raw sampled value for a parameter, optionally extract the actual desired value. + + By default just passes the value through unchanged. + """ + return value + + +class ExternalParameterConverter: + def __init__(self, samplerType, paramType): + self.samplerType = samplerType + self.paramType = paramType + self.externalParams = [] + + def registerExternalParams(self, dist): + for d in dist.recursiveDependencies(): + if isinstance(d, self.paramType): + self.externalParams.append(d) + + def convert(self, samp): + if not isinstance(samp, Samplable): + return + + sampCond = samp._conditioned + + if isinstance(sampCond, self.paramType) or not isinstance(sampCond, Distribution): + for dep in sampCond._dependencies: + self.convert(dep) + else: + try: + self.convertInner(samp) + except NotImplementedError: + warnings.warn( + f"Unable to convert {type(sampCond)} to {self.paramType} based distribution." + ) + + def convertInner(self, dist): + """Condition dist to an equivalent `ExternalParameter` based sampblable. + + If such a conversion is not possible, this function should raise NotImplementedError. + """ + raise NotImplementedError + + +class TimeSeriesParameter: + def __init__(self, callback): + self._callback = callback + self._lastSimulation = None + self._lastTime = -1 + + def getSample(self): + import scenic.syntax.veneer as veneer + + assert veneer.currentSimulation is not None + + if self._lastSimulation is not veneer.currentSimulation: + self._lastSimulation = veneer.currentSimulation + self._lastTime = -1 + + if veneer.currentSimulation.currentTime <= self._lastTime: + raise RuntimeError( + "Attempted `getSample` for a TimeSeries external parameter twice in one timestep." + ) + + self._lastTime = veneer.currentSimulation.currentTime + return self._callback() + + +def TimeSeries(param): + if not isinstance(param, ExternalParameter): + raise TypeError("Cannot turn a non `ExternalParameter` into a time series") + + param.isTimeSeries = True + return param diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py new file mode 100644 index 000000000..1521583cd --- /dev/null +++ b/src/scenic/core/external_params/optuna.py @@ -0,0 +1,198 @@ +from abc import abstractmethod +import math + +import optuna + +from scenic.core.distributions import ( + DiscreteRange, + Normal, + Options, + Range, + toDistribution, +) +from scenic.core.external_params.external_params import * +from scenic.core.lazy_eval import valueInContext +from scenic.core.regions import PointInRegionDistribution +import scenic.syntax.veneer + + +class OptunaSampler(ExternalSampler): + def __init__(self, params, globalParams): + super().__init__(params, globalParams) + + optuna.logging.set_verbosity( + globalParams.get("optunaLogLevel", optuna.logging.WARNING) + ) + + self.sampler = globalParams.get("optunaSampler", None) + self.study_name = globalParams.get("optunaStudyName", None) + self.storage = globalParams.get("optunaStorage", None) + self.study = optuna.create_study( + sampler=self.sampler, + study_name=self.study_name, + pruner=optuna.pruners.NopPruner(), + storage=self.storage, + direction="minimize", + ) + self.params = tuple(params) + for index, param in enumerate(self.params): + if not isinstance(param, OptunaParameter): + raise RuntimeError( + f"OptunaSampler given parameter of wrong type: {param}" + ) + param.sampler = self + param.index = index + + @classmethod + def getExternalParamType(cls): + return OptunaParameter + + @classmethod + def getExternalParameterConverter(cls): + return OptunaParameterConverter(cls, cls.getExternalParamType()) + + @property + def trial(self): + return self.cachedSample + + def nextSample(self, feedback): + if feedback is not None: + if self.study_name is None: + warnings.warn( + "Feedback passed without setting Optuna study name. Feedback will not have any effect." + ) + assert self.trial is not None + self.study.tell(self.trial, feedback) + + return self.study.ask() + + def valueFor(self, param): + if param.isTimeSeries: + raise ValueError( + "OptunaSampler does currently support timeSeries parameters." + ) + + return param.suggestValue(self) + + +class OptunaParameterConverter(ExternalParameterConverter): + def convertInner(self, dist): + distCond = dist._conditioned + if isinstance(distCond, Range): + self.convert(distCond.low) + self.convert(distCond.high) + newDist = OptunaRange(distCond.low, distCond.high) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, DiscreteRange): + self.convert(distCond.low) + self.convert(distCond.high) + newDist = OptunaDiscreteRange(distCond.low, distCond.high) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, Options): + for o in distCond.options: + self.convert(o) + newDist = OptunaOptions(distCond.options) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, Normal): + self.convert(distCond.stddev) + self.convert(distCond.mean) + newDist = Normal.cdfinv(distCond.mean, distCond.stddev, OptunaRange(-1, 1)) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif ( + isinstance(distCond, PointInRegionDistribution) + and not distCond._deterministic + ): + self.convert(distCond.region) + + # def makeSampleVals(dims): + # if isinstance(dims, tuple): + # return toDistribution(tuple(makeSampleVals(dim) for dim in dims)) + # elif isinstance(dims, int): + # return toDistribution(tuple(OptunaRange(0,1) for _ in range(dims))) + # else: + # assert False, dims + + # sampleVals = makeSampleVals(dist.region._sampleVals) + sampleVals = toDistribution(tuple(OptunaRange(0, 1) for _ in range(3))) + self.registerExternalParams(sampleVals) + newDist = PointInRegionDistribution( + region=distCond.region, tag=distCond.tag, sampleVals=sampleVals + ) + dist.conditionTo(newDist) + elif distCond._deterministic: + for dep in distCond._dependencies: + self.convert(dep) + else: + raise NotImplementedError + + +class OptunaParameter(ExternalParameter): + def __init__(self): + super().__init__() + self.index = None + + @abstractmethod + def suggestValue(self, trial): + pass + + @property + def optunaName( + self, + ): + assert self.index is not None + return f"{type(self)}_{self.index}" + + +class OptunaRange(OptunaParameter): + """A :obj:`~scenic.core.distributions.Range` (real interval) sampled by VerifAI.""" + + _defaultValueType = float + + def __init__(self, low, high): + super().__init__() + self.low = low + self.high = high + + def suggestValue(self, sampler): + return sampler.trial.suggest_float(self.optunaName, self.low, self.high) + + +class OptunaDiscreteRange(OptunaParameter): + """A :obj:`~scenic.core.distributions.DiscreteRange` (integer interval) sampled by Optuna.""" + + _defaultValueType = int + + def __init__(self, low, high): + super().__init__() + self.low = low + self.high = high + + def suggestValue(self, sampler): + return sampler.trial.suggest_int(self.optunaName, self.low, self.high) + + +class _OptunaCategoricalHelper(OptunaParameter): + _defaultValueType = int + + def __init__(self, numOptions): + super().__init__() + self.numOptions = numOptions + + def suggestValue(self, sampler): + return sampler.trial.suggest_categorical( + self.optunaName, list(range(self.numOptions + 1)) + ) + + +class OptunaOptions(Options): + """An :obj:`~scenic.core.distributions.Options` (discrete set) sampled by Optuna.""" + + @staticmethod + def makeSelector(n, weights): + if weights: + warnings.warn("Ignoring weights passed to OptunaOptions.") + return _OptunaCategoricalHelper(n) diff --git a/src/scenic/core/external_params/verifai.py b/src/scenic/core/external_params/verifai.py new file mode 100644 index 000000000..3b25249ef --- /dev/null +++ b/src/scenic/core/external_params/verifai.py @@ -0,0 +1,263 @@ +from scenic.core.distributions import Options +from scenic.core.external_params.external_params import * + + +class VerifaiSampler(ExternalSampler): + """An external sampler exposing the samplers in the VerifAI toolkit. + + The sampler can be configured using the following Scenic :term:`global parameters`: + + * ``verifaiSamplerType`` -- sampler type (see the ``verifai.server.choose_sampler`` + function); the default is ``'halton'`` + * ``verifaiSamplerParams`` -- ``DotMap`` of options passed to the sampler + + The `VerifaiSampler` supports external parameters which are instances of `VerifaiParameter`. + """ + + def __init__(self, params, globalParams): + super().__init__(params, globalParams) + import verifai.features + import verifai.server + + self._verifaiDynamic = int(metadata.version("verifai").split(".")[0]) > 2 + + # construct FeatureSpace + timeBound = globalParams.get("timeBound", 0) + usingProbs = False + self.params = tuple(params) + for index, param in enumerate(self.params): + if not isinstance(param, VerifaiParameter): + raise RuntimeError( + f"VerifaiSampler given parameter of wrong type: {param}" + ) + param.sampler = self + param.index = index + if param.probs is not None: + usingProbs = True + + if not self._verifaiDynamic and any(param.isTimeSeries for param in self.params): + raise RuntimeError("TimeSeries not supported for VerifAI versions < 3.0") + + if timeBound == 0 and any(param.isTimeSeries for param in self.params): + warnings.warn( + "TimeSeries external parameter used but no global parameter `timeBound` is specified. " + "(If using VerifAI’s ScenicSampler, set its maxSteps option)." + ) + + fs_kwargs = {} + if self._verifaiDynamic: + fs_kwargs["timeBound"] = timeBound + + space = verifai.features.FeatureSpace( + { + self.nameForParam(index): ( + verifai.features.Feature(param.domain) + if not param.isTimeSeries + else verifai.features.TimeSeriesFeature(param.domain) + ) + for index, param in enumerate(self.params) + }, + **fs_kwargs, + ) + + # set up VerifAI sampler + samplerType = globalParams.get("verifaiSamplerType", "halton") + samplerParams = globalParams.get("verifaiSamplerParams", None) + if usingProbs and samplerType == "ce": + if samplerParams is None: + samplerParams = DotMap() + else: + samplerParams = samplerParams.copy() # avoid mutating original + if "cont" in samplerParams or "disc" in samplerParams: + raise RuntimeError( + "CE distributions specified in both VerifaiParameters" + " and verifaiSamplerParams" + ) + cont_buckets = [] + cont_dists = [] + disc_dists = [] + for param in self.params: + if isinstance(param, VerifaiRange): + if param.probs is None: + buckets = 5 + dist = numpy.ones(buckets) / buckets + else: + dist = numpy.array(param.probs) + buckets = len(dist) + cont_buckets.append(buckets) + cont_dists.append(dist) + elif isinstance(param, VerifaiDiscreteRange): + n = param.high - param.low + 1 + dist = ( + numpy.ones(n) / n + if param.probs is None + else numpy.array(param.probs) + ) + disc_dists.append(dist) + else: + raise RuntimeError(f"Parameter {param} not supported by CE sampler") + samplerParams.cont.buckets = cont_buckets + samplerParams.cont.dist = numpy.array(cont_dists) + samplerParams.disc.dist = numpy.array(disc_dists) + data = verifai.server.choose_sampler( + space, samplerType, sampler_params=samplerParams + ) + if not data: + raise RuntimeError(f'Unknown VerifAI sampler type "{samplerType}"') + self.sampler = data[1] + + # default rejection feedback is positive so cross-entropy sampler won't update; + # for other active samplers an appropriate value should be set manually + if self.rejectionFeedback is None: + self.rejectionFeedback = 1 + self.cachedSample = None + + self._lastSample = None + self._lastInfo = None + self._lastDynamicSample = None + self._lastSimulation = None + self._lastTime = -1 + + def nextSample(self, feedback): + if feedback is not None: + assert self._lastSample is not None + if self._verifaiDynamic: + self._lastSample.complete(feedback) + else: + self.sampler.update(self._lastSample, self._lastInfo, feedback) + + if self._verifaiDynamic: + self._lastSample = self.sampler.getSample() + else: + lastSample = self.sampler.getSample() + self._lastSample = lastSample[0] + self._lastInfo = lastSample[1] + return self._lastSample + + def nextDynamicSample(self): + import scenic.syntax.veneer as veneer + + assert veneer.currentSimulation is not None + + if self._lastSimulation is not veneer.currentSimulation: + self._lastSimulation = veneer.currentSimulation + self._lastTime = -1 + + if veneer.currentSimulation.currentTime > self._lastTime: + feedback = veneer.currentSimulation + self._lastDynamicSample = self.cachedSample.getDynamicSample(feedback) + self._lastTime = veneer.currentSimulation.currentTime + + return self._lastDynamicSample + + def valueFor(self, param): + if not param.isTimeSeries: + if self._verifaiDynamic: + sampleTarget = self.cachedSample.staticSample + else: + sampleTarget = self.cachedSample + return param.extractOutput( + getattr(sampleTarget, self.nameForParam(param.index)) + ) + else: + callback = lambda: param.extractOutput( + getattr( + self.nextDynamicSample(), + self.nameForParam(param.index), + ) + ) + return TimeSeriesParameter(callback) + + @staticmethod + def nameForParam(i): + """Parameter name for a given index in the Feature Space.""" + return f"param{i}" + + +class VerifaiParameter(ExternalParameter): + """An external parameter sampled using one of VerifAI's samplers.""" + + def __init__(self, domain): + super().__init__() + self.domain = domain + + @staticmethod + def withPrior(dist, buckets=None): + """Creates a `VerifaiParameter` using the given distribution as a prior. + + Since the VerifAI cross-entropy sampler currently only supports piecewise-constant + distributions, if the prior is not of that form it may be approximated. For most + built-in distributions, the approximation is exact: for a particular distribution, + check its `bucket` method. + """ + if not dist.isPrimitive: + raise RuntimeError( + "VerifaiParameter.withPrior called on " + f"non-primitive distribution {dist}" + ) + bucketed = dist.bucket(buckets=buckets) + return VerifaiOptions( + bucketed.optWeights if bucketed.optWeights else bucketed.options + ) + + +class VerifaiRange(VerifaiParameter): + """A :obj:`~scenic.core.distributions.Range` (real interval) sampled by VerifAI.""" + + _defaultValueType = float + + def __init__(self, low, high, buckets=None, weights=None): + import verifai.features + + super().__init__(verifai.features.Box([low, high])) + if weights is not None: + weights = tuple(weights) + if buckets is not None and len(weights) != buckets: + raise RuntimeError( + f"VerifaiRange created with {len(weights)} weights " + f"but {buckets} buckets" + ) + elif buckets is not None: + weights = [1] * buckets + else: + self.probs = None + return + total = sum(weights) + self.probs = tuple(wt / total for wt in weights) + + def extractOutput(self, value): + assert len(value) == 1 + return value[0] + + +class VerifaiDiscreteRange(VerifaiParameter): + """A :obj:`~scenic.core.distributions.DiscreteRange` (integer interval) sampled by VerifAI.""" + + _defaultValueType = float + + def __init__(self, low, high, weights=None): + import verifai.features + + super().__init__(verifai.features.DiscreteBox([low, high])) + if weights is not None: + if len(weights) != (high - low + 1): + raise RuntimeError( + f"VerifaiDiscreteRange created with {len(weights)} weights " + f"for {high - low + 1} values" + ) + total = sum(weights) + self.probs = tuple(wt / total for wt in weights) + else: + self.probs = None + + def extractOutput(self, value): + assert len(value) == 1 + return value[0] + + +class VerifaiOptions(Options): + """An :obj:`~scenic.core.distributions.Options` (discrete set) sampled by VerifAI.""" + + @staticmethod + def makeSelector(n, weights): + return VerifaiDiscreteRange(0, n, weights) diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index 2f2285e66..d733d2bd3 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -41,6 +41,7 @@ from scenic.core.dynamics.scenarios import DynamicScenario import scenic.core.errors as errors from scenic.core.errors import InvalidScenarioError, PythonCompileError +from scenic.core.external_params import ExternalParameter, ExternalSampler from scenic.core.lazy_eval import needsLazyEvaluation import scenic.core.pruning as pruning from scenic.core.serialization import deterministicHash @@ -708,7 +709,6 @@ def isModularScenario(thing): # Convert distributions to ExternalParameters if requested, and create # the external sampler. if scenario.params.get("convertDistributions", False): - from scenic.core.external_params import ExternalParameter from scenic.core.external_params.verifai import VerifaiSampler externalParamConverter = scenario.params.get( diff --git a/src/scenic/syntax/veneer.py b/src/scenic/syntax/veneer.py index 357a70fb2..ed9b3affe 100644 --- a/src/scenic/syntax/veneer.py +++ b/src/scenic/syntax/veneer.py @@ -215,7 +215,7 @@ ) from scenic.core.dynamics.invocables import BlockConclusion, runTryInterrupt from scenic.core.dynamics.scenarios import DynamicScenario -from scenic.core.external_params.external_params import TimeSeries +from scenic.core.external_params import TimeSeries from scenic.core.external_params.optuna import ( OptunaDiscreteRange, OptunaOptions, From c08b509b92894f82490d2cf797f56d84667f3b9a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 13:20:34 -0700 Subject: [PATCH 120/134] Fixed random dependencies in external sampler --- .../core/external_params/external_params.py | 4 +-- src/scenic/core/external_params/optuna.py | 35 +++++++++++++------ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/src/scenic/core/external_params/external_params.py b/src/scenic/core/external_params/external_params.py index d9ebf4383..9ee3166d4 100644 --- a/src/scenic/core/external_params/external_params.py +++ b/src/scenic/core/external_params/external_params.py @@ -191,8 +191,8 @@ def valueFor(self, param): class ExternalParameter(Distribution): """A value determined by external code rather than Scenic's internal sampler.""" - def __init__(self): - super().__init__() + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) self.sampler = None self.isTimeSeries = False import scenic.syntax.veneer as veneer # TODO improve? diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 1521583cd..a4eaa3ec6 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -66,13 +66,13 @@ def nextSample(self, feedback): return self.study.ask() - def valueFor(self, param): + def valueFor(self, param, value): if param.isTimeSeries: raise ValueError( "OptunaSampler does currently support timeSeries parameters." ) - return param.suggestValue(self) + return param.suggestValue(self, value) class OptunaParameterConverter(ExternalParameterConverter): @@ -131,10 +131,19 @@ def convertInner(self, dist): class OptunaParameter(ExternalParameter): - def __init__(self): - super().__init__() + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) self.index = None + def sampleGiven(self, value): + """Specialization of `Samplable.sampleGiven` for external parameters. + + By default, this method simply looks up the value previously sampled by + `ExternalSampler.sample`. + """ + assert self.sampler is not None + return self.sampler.valueFor(self, value) + @abstractmethod def suggestValue(self, trial): pass @@ -153,12 +162,14 @@ class OptunaRange(OptunaParameter): _defaultValueType = float def __init__(self, low, high): - super().__init__() + super().__init__(low, high) self.low = low self.high = high - def suggestValue(self, sampler): - return sampler.trial.suggest_float(self.optunaName, self.low, self.high) + def suggestValue(self, sampler, value): + return sampler.trial.suggest_float( + self.optunaName, value[self.low], value[self.high] + ) class OptunaDiscreteRange(OptunaParameter): @@ -167,12 +178,14 @@ class OptunaDiscreteRange(OptunaParameter): _defaultValueType = int def __init__(self, low, high): - super().__init__() + super().__init__(low, high) self.low = low self.high = high - def suggestValue(self, sampler): - return sampler.trial.suggest_int(self.optunaName, self.low, self.high) + def suggestValue(self, sampler, value): + return sampler.trial.suggest_int( + self.optunaName, value[self.low], value[self.high] + ) class _OptunaCategoricalHelper(OptunaParameter): @@ -182,7 +195,7 @@ def __init__(self, numOptions): super().__init__() self.numOptions = numOptions - def suggestValue(self, sampler): + def suggestValue(self, sampler, _): return sampler.trial.suggest_categorical( self.optunaName, list(range(self.numOptions + 1)) ) From 4c2a70885b9b2342c2836d43ce3d5cc5a6fef2ea Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 14:08:29 -0700 Subject: [PATCH 121/134] Added external sampler support for Uniform star dists. --- src/scenic/core/external_params/optuna.py | 38 +++++++++++++++-------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index a4eaa3ec6..f288e85e4 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -8,6 +8,8 @@ Normal, Options, Range, + RejectionException, + UniformDistribution, toDistribution, ) from scenic.core.external_params.external_params import * @@ -90,18 +92,26 @@ def convertInner(self, dist): newDist = OptunaDiscreteRange(distCond.low, distCond.high) self.registerExternalParams(newDist) dist.conditionTo(newDist) - elif isinstance(distCond, Options): - for o in distCond.options: - self.convert(o) - newDist = OptunaOptions(distCond.options) - self.registerExternalParams(newDist) - dist.conditionTo(newDist) elif isinstance(distCond, Normal): self.convert(distCond.stddev) self.convert(distCond.mean) newDist = Normal.cdfinv(distCond.mean, distCond.stddev, OptunaRange(-1, 1)) self.registerExternalParams(newDist) dist.conditionTo(newDist) + elif isinstance(distCond, Options): + for o in distCond.options: + self.convert(o) + newDist = OptunaOptions(distCond.options) + self.registerExternalParams(newDist) + dist.conditionTo(newDist) + elif isinstance(distCond, UniformDistribution): + for o in distCond.options: + self.convert(o) + newSelector = _OptunaCategoricalHelper( + distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." + ) + self.registerExternalParams(newSelector) + distCond.selector.conditionTo(newSelector) elif ( isinstance(distCond, PointInRegionDistribution) and not distCond._deterministic @@ -191,14 +201,16 @@ def suggestValue(self, sampler, value): class _OptunaCategoricalHelper(OptunaParameter): _defaultValueType = int - def __init__(self, numOptions): - super().__init__() + def __init__(self, numOptions, emptyMessage): + super().__init__(numOptions) self.numOptions = numOptions + self.emptyMessage = emptyMessage - def suggestValue(self, sampler, _): - return sampler.trial.suggest_categorical( - self.optunaName, list(range(self.numOptions + 1)) - ) + def suggestValue(self, sampler, value): + optionsNums = list(range(value[self.numOptions] + 1)) + if len(optionsNums) == 0: + raise RejectionException(self.emptyMessage) + return sampler.trial.suggest_categorical(self.optunaName, optionsNums) class OptunaOptions(Options): @@ -208,4 +220,4 @@ class OptunaOptions(Options): def makeSelector(n, weights): if weights: warnings.warn("Ignoring weights passed to OptunaOptions.") - return _OptunaCategoricalHelper(n) + return _OptunaCategoricalHelper(n, emptyMessage="Empty Optuna Options.") From a519d7167d849cdc84e1a2224244ef138d6c412a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 14:55:09 -0700 Subject: [PATCH 122/134] Dusabled translation case due to Optuna limitation. --- src/scenic/core/external_params/optuna.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index f288e85e4..27ed0af72 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -104,14 +104,14 @@ def convertInner(self, dist): newDist = OptunaOptions(distCond.options) self.registerExternalParams(newDist) dist.conditionTo(newDist) - elif isinstance(distCond, UniformDistribution): - for o in distCond.options: - self.convert(o) - newSelector = _OptunaCategoricalHelper( - distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." - ) - self.registerExternalParams(newSelector) - distCond.selector.conditionTo(newSelector) + # elif isinstance(distCond, UniformDistribution): + # for o in distCond.options: + # self.convert(o) + # newSelector = _OptunaCategoricalHelper( + # distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." + # ) + # self.registerExternalParams(newSelector) + # distCond.selector.conditionTo(newSelector) elif ( isinstance(distCond, PointInRegionDistribution) and not distCond._deterministic From 69362d5657f5ed5541f44afee9677a82e41a2c89 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 14:57:35 -0700 Subject: [PATCH 123/134] More fixes. --- src/scenic/core/external_params/optuna.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 27ed0af72..2b7e03539 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -187,12 +187,15 @@ class OptunaDiscreteRange(OptunaParameter): _defaultValueType = int - def __init__(self, low, high): + def __init__(self, low, high, emptyMessage=None): super().__init__(low, high) self.low = low self.high = high + self.emptyMessage def suggestValue(self, sampler, value): + if value[self.low] > value[self.high]: + raise RejectionException(self.emptyMessage if self.emptyMessage else "") return sampler.trial.suggest_int( self.optunaName, value[self.low], value[self.high] ) From 105933b8dcb667a35c89c2f3ef15c1b890f6145b Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 14:58:37 -0700 Subject: [PATCH 124/134] Fix typo. --- src/scenic/core/external_params/optuna.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 2b7e03539..7d5e5d174 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -191,7 +191,7 @@ def __init__(self, low, high, emptyMessage=None): super().__init__(low, high) self.low = low self.high = high - self.emptyMessage + self.emptyMessage = emptyMessage def suggestValue(self, sampler, value): if value[self.low] > value[self.high]: From ae9396623b49624eecc1caab7ed9084cff7d1c1e Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 15:06:04 -0700 Subject: [PATCH 125/134] Debug. --- src/scenic/core/external_params/external_params.py | 2 ++ src/scenic/core/external_params/optuna.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/scenic/core/external_params/external_params.py b/src/scenic/core/external_params/external_params.py index 9ee3166d4..b182f18ae 100644 --- a/src/scenic/core/external_params/external_params.py +++ b/src/scenic/core/external_params/external_params.py @@ -245,6 +245,8 @@ def convert(self, samp): f"Unable to convert {type(sampCond)} to {self.paramType} based distribution." ) + print(f"{samp}: {len(self.externalParams)}") + def convertInner(self, dist): """Condition dist to an equivalent `ExternalParameter` based sampblable. diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 7d5e5d174..d5dfcbd2a 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -125,8 +125,8 @@ def convertInner(self, dist): # return toDistribution(tuple(OptunaRange(0,1) for _ in range(dims))) # else: # assert False, dims - # sampleVals = makeSampleVals(dist.region._sampleVals) + sampleVals = toDistribution(tuple(OptunaRange(0, 1) for _ in range(3))) self.registerExternalParams(sampleVals) newDist = PointInRegionDistribution( From 4d695563f54f7eb353eeb89d6635998192363c50 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 15:06:44 -0700 Subject: [PATCH 126/134] More debug. --- src/scenic/core/external_params/external_params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scenic/core/external_params/external_params.py b/src/scenic/core/external_params/external_params.py index b182f18ae..42bbc0f28 100644 --- a/src/scenic/core/external_params/external_params.py +++ b/src/scenic/core/external_params/external_params.py @@ -245,7 +245,7 @@ def convert(self, samp): f"Unable to convert {type(sampCond)} to {self.paramType} based distribution." ) - print(f"{samp}: {len(self.externalParams)}") + print(f"{id(samp)}|{type(samp)}: {len(self.externalParams)}") def convertInner(self, dist): """Condition dist to an equivalent `ExternalParameter` based sampblable. From b954a43c199aae4840facb4f826c43731fc2e81e Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 15:10:54 -0700 Subject: [PATCH 127/134] Attempt fix. --- .../core/external_params/external_params.py | 2 -- src/scenic/core/external_params/optuna.py | 18 ++++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/scenic/core/external_params/external_params.py b/src/scenic/core/external_params/external_params.py index 42bbc0f28..9ee3166d4 100644 --- a/src/scenic/core/external_params/external_params.py +++ b/src/scenic/core/external_params/external_params.py @@ -245,8 +245,6 @@ def convert(self, samp): f"Unable to convert {type(sampCond)} to {self.paramType} based distribution." ) - print(f"{id(samp)}|{type(samp)}: {len(self.externalParams)}") - def convertInner(self, dist): """Condition dist to an equivalent `ExternalParameter` based sampblable. diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index d5dfcbd2a..3f782ca4b 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -104,14 +104,16 @@ def convertInner(self, dist): newDist = OptunaOptions(distCond.options) self.registerExternalParams(newDist) dist.conditionTo(newDist) - # elif isinstance(distCond, UniformDistribution): - # for o in distCond.options: - # self.convert(o) - # newSelector = _OptunaCategoricalHelper( - # distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." - # ) - # self.registerExternalParams(newSelector) - # distCond.selector.conditionTo(newSelector) + elif isinstance(distCond, UniformDistribution): + for o in distCond.options: + self.convert(o) + newSelector = OptunaDiscreteRange( + 0, + distCond.selector.high, + emptyMessage="Empty Optuna UniformDistribution.", + ) + self.registerExternalParams(newSelector) + distCond.selector.conditionTo(newSelector) elif ( isinstance(distCond, PointInRegionDistribution) and not distCond._deterministic From 12652aa57ba06730c974918a8177f91a2716bdff Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 15:16:58 -0700 Subject: [PATCH 128/134] Fixed leak. --- src/scenic/core/external_params/optuna.py | 20 +++++++++----------- src/scenic/syntax/translator.py | 2 ++ 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 3f782ca4b..428b61c8f 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -98,22 +98,20 @@ def convertInner(self, dist): newDist = Normal.cdfinv(distCond.mean, distCond.stddev, OptunaRange(-1, 1)) self.registerExternalParams(newDist) dist.conditionTo(newDist) - elif isinstance(distCond, Options): + elif isinstance(distCond, Options) and not isinstance(distCond, OptunaOptions): for o in distCond.options: self.convert(o) newDist = OptunaOptions(distCond.options) self.registerExternalParams(newDist) dist.conditionTo(newDist) - elif isinstance(distCond, UniformDistribution): - for o in distCond.options: - self.convert(o) - newSelector = OptunaDiscreteRange( - 0, - distCond.selector.high, - emptyMessage="Empty Optuna UniformDistribution.", - ) - self.registerExternalParams(newSelector) - distCond.selector.conditionTo(newSelector) + # elif isinstance(distCond, UniformDistribution): + # for o in distCond.options: + # self.convert(o) + # newSelector = _OptunaCategoricalHelper( + # distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." + # ) + # self.registerExternalParams(newSelector) + # distCond.selector.conditionTo(newSelector) elif ( isinstance(distCond, PointInRegionDistribution) and not distCond._deterministic diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index d733d2bd3..4ebbed6d3 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -740,6 +740,8 @@ def isModularScenario(thing): else: newExternalParams = [] + breakpoint() + scenario.createExternalSampler(newExternalParams) return scenario From f73fdb7515228fbf9484f96283da8b14881d007a Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 15:17:27 -0700 Subject: [PATCH 129/134] Remove breakpoint. --- src/scenic/syntax/translator.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/scenic/syntax/translator.py b/src/scenic/syntax/translator.py index 4ebbed6d3..d733d2bd3 100644 --- a/src/scenic/syntax/translator.py +++ b/src/scenic/syntax/translator.py @@ -740,8 +740,6 @@ def isModularScenario(thing): else: newExternalParams = [] - breakpoint() - scenario.createExternalSampler(newExternalParams) return scenario From 34098b11ea4fb5ca23c9eac9f1c9ff1cde09cf3b Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Fri, 24 Jul 2026 15:39:10 -0700 Subject: [PATCH 130/134] Attempting to fix optuna multivariate. --- src/scenic/core/external_params/optuna.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 428b61c8f..7a56be120 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -158,12 +158,9 @@ def sampleGiven(self, value): def suggestValue(self, trial): pass - @property - def optunaName( - self, - ): + def optunaName(self, extra=None): assert self.index is not None - return f"{type(self)}_{self.index}" + return f"{type(self).__name__}_{self.index}_{extra}" class OptunaRange(OptunaParameter): @@ -178,7 +175,7 @@ def __init__(self, low, high): def suggestValue(self, sampler, value): return sampler.trial.suggest_float( - self.optunaName, value[self.low], value[self.high] + self.optunaName(), value[self.low], value[self.high] ) @@ -197,7 +194,7 @@ def suggestValue(self, sampler, value): if value[self.low] > value[self.high]: raise RejectionException(self.emptyMessage if self.emptyMessage else "") return sampler.trial.suggest_int( - self.optunaName, value[self.low], value[self.high] + self.optunaName((self.low, self.high)), value[self.low], value[self.high] ) @@ -213,7 +210,7 @@ def suggestValue(self, sampler, value): optionsNums = list(range(value[self.numOptions] + 1)) if len(optionsNums) == 0: raise RejectionException(self.emptyMessage) - return sampler.trial.suggest_categorical(self.optunaName, optionsNums) + return sampler.trial.suggest_categorical(self.optunaName(), optionsNums) class OptunaOptions(Options): From 979fee484ea957470da9ea3bd91dd428a7d8ab18 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 27 Jul 2026 10:40:15 -0700 Subject: [PATCH 131/134] Options now use Optuna categorical --- src/scenic/core/external_params/optuna.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 7a56be120..a9f1ab2cf 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -104,14 +104,15 @@ def convertInner(self, dist): newDist = OptunaOptions(distCond.options) self.registerExternalParams(newDist) dist.conditionTo(newDist) - # elif isinstance(distCond, UniformDistribution): - # for o in distCond.options: - # self.convert(o) - # newSelector = _OptunaCategoricalHelper( - # distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." - # ) - # self.registerExternalParams(newSelector) - # distCond.selector.conditionTo(newSelector) + elif isinstance(distCond, UniformDistribution): + breakpoint() + for o in distCond.options: + self.convert(o) + newSelector = _OptunaCategoricalHelper( + distCond.selector.high, emptyMessage="Empty Optuna UniformDistribution." + ) + self.registerExternalParams(newSelector) + distCond.selector.conditionTo(newSelector) elif ( isinstance(distCond, PointInRegionDistribution) and not distCond._deterministic @@ -210,7 +211,9 @@ def suggestValue(self, sampler, value): optionsNums = list(range(value[self.numOptions] + 1)) if len(optionsNums) == 0: raise RejectionException(self.emptyMessage) - return sampler.trial.suggest_categorical(self.optunaName(), optionsNums) + return sampler.trial.suggest_categorical( + self.optunaName(self.numOptions), optionsNums + ) class OptunaOptions(Options): From 916b64bafbed7160d569b14d376467b921f33096 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 27 Jul 2026 10:40:54 -0700 Subject: [PATCH 132/134] Remove breakpoint. --- src/scenic/core/external_params/optuna.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index a9f1ab2cf..63d10ab50 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -105,7 +105,6 @@ def convertInner(self, dist): self.registerExternalParams(newDist) dist.conditionTo(newDist) elif isinstance(distCond, UniformDistribution): - breakpoint() for o in distCond.options: self.convert(o) newSelector = _OptunaCategoricalHelper( From 4a4740afce4dc8427bade19fa6283ea88e2505b9 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 27 Jul 2026 10:42:59 -0700 Subject: [PATCH 133/134] Properly use sampled value in Optuna names. --- src/scenic/core/external_params/optuna.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/scenic/core/external_params/optuna.py b/src/scenic/core/external_params/optuna.py index 63d10ab50..6220b4c15 100644 --- a/src/scenic/core/external_params/optuna.py +++ b/src/scenic/core/external_params/optuna.py @@ -191,10 +191,12 @@ def __init__(self, low, high, emptyMessage=None): self.emptyMessage = emptyMessage def suggestValue(self, sampler, value): - if value[self.low] > value[self.high]: + if value[self.low] > value[self.low]: raise RejectionException(self.emptyMessage if self.emptyMessage else "") return sampler.trial.suggest_int( - self.optunaName((self.low, self.high)), value[self.low], value[self.high] + self.optunaName((value[self.low], value[self.low])), + value[self.low], + value[self.high], ) @@ -211,7 +213,7 @@ def suggestValue(self, sampler, value): if len(optionsNums) == 0: raise RejectionException(self.emptyMessage) return sampler.trial.suggest_categorical( - self.optunaName(self.numOptions), optionsNums + self.optunaName(value[self.numOptions]), optionsNums ) From cefd7ec37b3e29e7f2325d6fd500119245a76b37 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Mon, 27 Jul 2026 14:33:20 -0700 Subject: [PATCH 134/134] Polygon sampling improvements. --- src/scenic/core/regions.py | 39 +++++++++++++++++++++++--------------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/src/scenic/core/regions.py b/src/scenic/core/regions.py index 7e43fb55b..1151658d8 100644 --- a/src/scenic/core/regions.py +++ b/src/scenic/core/regions.py @@ -3181,27 +3181,36 @@ def _samplingData(self): return trianglesAndBounds, cumulativeTriangleAreas def uniformPointInner(self): - trisAndBounds, cumulativeAreas = self._samplingData - triangle, bounds = random.choices(trisAndBounds, cum_weights=cumulativeAreas)[0] - minx, miny, maxx, maxy = bounds - # TODO improve? - while True: - x, y = random.uniform(minx, maxx), random.uniform(miny, maxy) - if shapely.intersects_xy(triangle, x, y): - return self.orient(Vector(x, y, self.z)) + return self.parameterizedUniformPointInner( + tuple(random.random() for _ in range(3)) + ) + # trisAndBounds, cumulativeAreas = self._samplingData + # triangle, bounds = random.choices(trisAndBounds, cum_weights=cumulativeAreas)[0] + # minx, miny, maxx, maxy = bounds + # # TODO improve? + # while True: + # x, y = random.uniform(minx, maxx), random.uniform(miny, maxy) + # if shapely.intersects_xy(triangle, x, y): + # return self.orient(Vector(x, y, self.z)) def parameterizedUniformPointInner(self, vals): # assert len(vals) == 3 trisAndBounds, cumulativeAreas = self._samplingData - triangle, bounds = trisAndBounds[randomIndexFromVal(vals[0], cumulativeAreas)] - minx, miny, maxx, maxy = bounds + triangle, _ = trisAndBounds[randomIndexFromVal(vals[0], cumulativeAreas)] - x, y = minx + vals[1] * (maxx - minx), miny + vals[2] * (maxy - miny) - if shapely.intersects_xy(triangle, x, y): - return self.orient(Vector(x, y, self.z)) - else: - raise RejectionException + # Pick a point in the parallelogram of this triangle and its reflection + p1, p2, p3 = [Vector(*p) for p in triangle.exterior.coords[:3]] + v1 = p2 - p1 + v2 = p3 - p1 + pt = p1 + vals[1] * v1 + vals[2] * v2 + + # If the point is in the triangle's reflection, reflect it back. + if not triangle.contains(toShapely(pt)): + midpoint = (p3 + p2) / 2 + pt = pt + 2 * (midpoint - pt) + + return self.orient(Vector(pt.x, pt.y, self.z)) @distributionFunction def intersects(self, other, triedReversed=False):