diff --git a/examples/contracts/dev.contract b/examples/contracts/dev.contract index 3e7c8a1c7..5e20d84f2 100644 --- a/examples/contracts/dev.contract +++ b/examples/contracts/dev.contract @@ -88,14 +88,11 @@ component PIDThrottleSystem(target_dist, max_speed): return {"throttle": float(throttle)} -component ThrottleSafetyFilter(min_dist, max_brake=5, buffer_padding=0): +component ThrottleSafetyFilter(min_dist, max_brake=5, buffer_padding=0, timestep=0.1, perception_distance=1000): """ A component that modulates actions, passing them through unchanged unless we are dangerously close to the car in front of us, in which case the actions are swapped to brake with maximum force. """ - sensors: - self.timestep: float as timestep - inputs: dist: float speed: float @@ -105,24 +102,19 @@ component ThrottleSafetyFilter(min_dist, max_brake=5, buffer_padding=0): modulated_throttle: float state: - last_dist: Union[None, float] = None + last_dist: float = 0.0 body: - # In first timestep, don't take any action - if state.last_dist is None: - state.last_dist = dist - return {"modulated_throttle": 0.0} - # If we are in the "danger zone", brake HARD. Otherwise, pass through the inputs actions action. - rel_speed = (state.last_dist - dist)/timestep - stopping_time = ceil(rel_speed/max_brake)+1 - rel_dist_covered = stopping_time*rel_speed - danger_dist = min_dist + buffer_padding + max(0, rel_dist_covered) + p_rel_speed = state.last_dist - dist + p_stopping_time = ceil(p_rel_speed/max_brake) + p_rel_dist_covered = p_stopping_time*p_rel_speed + p_danger_dist = min_dist + buffer_padding + max(0, p_rel_dist_covered) # Update last_dist state.last_dist = dist - if dist < danger_dist: + if dist < p_danger_dist: return {"modulated_throttle": -1.0} else: return {"modulated_throttle": float(throttle)} @@ -259,6 +251,13 @@ contract AccurateDistance(perception_distance, abs_dist_err=0.5): # distance is greater than our visible range. always ((not behind_car) implies (dist > perception_distance+abs_dist_err)) +contract AccurateSpeed(): + outputs: + speed: float + + guarantees: + always speed == self.speed + contract AccurateRelativeSpeeds(perception_distance, abs_dist_err=0.5, abs_speed_err=0.5, max_brake=float("inf"), max_accel=float("inf")): """ Contract proving that if we have access to relatively accurate distance, we can get a relatively accurate relative speed between the two cars (Assuming @@ -279,7 +278,7 @@ contract AccurateRelativeSpeeds(perception_distance, abs_dist_err=0.5, abs_speed behind_car = lead_dist <= perception_distance # Lead speed can be roughly calculated by comparing two timesteps - relative_speed = ((next dist) - dist)/self.timestep + relative_speed = ((next dist) - dist) true_relative_speed = self.speed - lead_car.speed assumptions: @@ -288,6 +287,9 @@ contract AccurateRelativeSpeeds(perception_distance, abs_dist_err=0.5, abs_speed always (behind_car implies (lead_dist-abs_dist_err <= dist <= lead_dist+abs_dist_err)) + # Assume that the we are not going to brake or accelerate with too much force. + always (-max_brake <= (next self.speed) - self.speed <= max_accel) + # Assume that the lead car is not going to brake or accelerate with too much force. always (-max_brake <= (next lead_car.speed) - lead_car.speed <= max_accel) @@ -306,23 +308,18 @@ contract SafeThrottleFilter(perception_distance, min_dist, max_speed, abs_dist_e modulated_throttle: float definitions: - cars = [obj for obj in objects if hasattr(obj, "isVehicle") and obj.isVehicle and obj.position is not self.position] - lead_distances = {car: leadDistance(self, car, workspace.network, maxDistance=2*perception_distance) for car in cars} - lead_car = sorted(cars, key=lambda x: lead_distances[x])[0] - # Lead speed can be roughly calculated by comparing two timesteps - relative_speed = ((next dist) - dist)/self.timestep if (next(next dist)) else ((next dist) - dist)/self.timestep - true_relative_speed = self.speed - lead_car.speed + p_relative_speed = (next dist) - dist - stopping_time = ceil(self.speed/max_brake) - rel_dist_covered = stopping_time * (true_relative_speed+abs_speed_err) - delta_stopping_time = ceil((self.speed+max_accel)/max_brake) - max_rdc_delta = delta_stopping_time * (true_relative_speed + max_brake + max_accel + abs_speed_err) - rel_dist_covered - buffer_dist = min_dist + (max(0, max_rdc_delta + rel_dist_covered)) + max_speed + 1 + p_stopping_time = ceil(speed/max_brake) + p_rel_dist_covered = p_stopping_time * (p_relative_speed+abs_speed_err) + p_delta_stopping_time = ceil((speed+max_accel)/max_brake) + p_max_rdc_delta = p_delta_stopping_time * (p_relative_speed + max_brake + max_accel + 2*abs_speed_err) - p_rel_dist_covered + p_buffer_dist = min_dist + (max(0, p_max_rdc_delta + p_rel_dist_covered)) + max_speed + 1 guarantees: # Guarantee that if we sense we are too close to the car in front of us, we brake with max force. - always (dist <= buffer_dist+abs_dist_err) implies (modulated_throttle == -1) + always (dist <= p_buffer_dist+abs_dist_err) implies (modulated_throttle == -1) contract PassthroughBrakingActionGenerator(): """ A contract stating that the outputted action represents the input throttle""" @@ -409,8 +406,8 @@ MIN_DIST = 5 MAX_BRAKE = 0.9 MAX_ACCEL = 0.5 PERCEPTION_DISTANCE = 1000 -ABS_DIST_ERR = 1 -ABS_SPEED_ERR = 0.4 +ABS_DIST_ERR = 0.1 +ABS_SPEED_ERR = 1.6 MAX_SPEED = 5.4 TARGET_DIST = 10 @@ -425,8 +422,10 @@ accurate_distance = test car.ps satisfies AccurateDistance( using SimulationTesting(scenario=ENVIRONMENT, maxSteps=6, confidence=0.95, batchSize=10, verbose=False), terminating after 100 samples +accurate_speed = assume car.sm satisfies AccurateSpeed() + # TODO: Replace with proof -accurate_speed = assume car.ps satisfies AccurateRelativeSpeeds( +accurate_rspeed = assume car.ps satisfies AccurateRelativeSpeeds( PERCEPTION_DISTANCE, abs_dist_err=ABS_DIST_ERR, abs_speed_err=ABS_SPEED_ERR, @@ -435,7 +434,7 @@ accurate_speed = assume car.ps satisfies AccurateRelativeSpeeds( ps_accuracy = compose over car.ps: use accurate_distance - use accurate_speed + use accurate_rspeed cs_safety = compose over car.cs: # TODO: Replace with proof @@ -455,6 +454,7 @@ max_braking_force = assume car.cc satisfies MaxBrakingForce(MAX_BRAKE), with cor keeps_distance_raw = compose over car: use ps_accuracy + use accurate_speed use cs_safety use max_braking_force diff --git a/src/scenic/contracts/refinement.py b/src/scenic/contracts/refinement.py index 8b09545f8..43fc967ea 100644 --- a/src/scenic/contracts/refinement.py +++ b/src/scenic/contracts/refinement.py @@ -112,6 +112,10 @@ def check(self, stmt, contract): prop_atomics = [a for a, _ in filter(lambda x: x[1] is bool, atomics)] real_atomics = [a for a, _ in filter(lambda x: x[1] is float, atomics)] + # Sort atomics lists to keep signal names from swapping + prop_atomics.sort(key=str) + real_atomics.sort(key=str) + ## Extract defs from all specs defs = {} for spec in i_assumptions + i_guarantees + tl_assumptions + tl_guarantees: