Updated relative speed contract and STF.

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
 

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: