Support ultimate bound

dtree_synth.py

@@ -38,7 +38,7 @@ def test_synth_dtree():
     """ Test using filtered data where we know an abstraction exists. """
 
     # Initialize Teacher
-    teacher = Teacher(norm_ord=1)
+    teacher = Teacher(norm_ord=1, ultimate_bound=0.25)
     teacher.set_old_state_bound(lb=[0.0, -1.0, 0.2], ub=[32.0, -0.9, 0.22])
 
     init_positive_examples, init_negative_examples = load_examples(

gem_stanley_teacher.py

@@ -88,7 +88,7 @@ class GEMStanleyDRealTeacher(DRealTeacherBase):
 class GEMStanleyGurobiTeacher(GurobiTeacherBase):
     # Ideal perception as a linear transform from state to ground truth percept
     PERC_GT = np.array([[0., -1., 0.],
-                    [0., 0., -1.]], float)
+                        [0., 0., -1.]], float)
 
     @staticmethod
     def is_spurious_example(state_dim: int, perc_dim: int, candidate: z3.BoolRef, cex: Tuple[float, ...]) -> bool:
@@ -104,15 +104,18 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
         else:
             raise RuntimeError(f"Cannot validate negative example {cex} by substitution")
 
-    def __init__(self, name="gem_stanley", norm_ord=2) -> None:
+    def __init__(self, name="gem_stanley", norm_ord=2, ultimate_bound: float=0.0) -> None:
+        assert ultimate_bound >= 0.0
+        self._ultimate_bound = ultimate_bound
+
         super().__init__(name=name,
                          state_dim=3, perc_dim=2, ctrl_dim=1, norm_ord=norm_ord)
 
     def is_positive_example(self, ex) -> bool:
         assert len(ex) == self.state_dim + self.perc_dim
 
-        def g(cte, phi):
-            error = phi + np.arctan(K_P*cte/FORWARD_VEL)
+        def g(cte, psi):
+            error = psi + np.arctan(K_P*cte/FORWARD_VEL)
             steer = np.clip(error, -STEERING_LIM, STEERING_LIM)
             return (steer,)
 
@@ -122,13 +125,16 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
             new_theta = theta + FORWARD_VEL*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
             return new_x, new_y, new_theta
 
-        def v(x, y, theta) -> float:
-            return np.linalg.norm([y, theta], ord=float(self._norm_ord))
+        def m_star(x, y, theta) -> Tuple[float, float]:
+            return -y, -theta
 
-        def spec(x, y, theta, d, phi) -> bool:
-            v_old = v(x, y, theta)
-            v_new = v(*f(x, y, theta, *g(d, phi)))
-            return v_new <= v_old
+        def v(cte, psi) -> float:
+            return np.linalg.norm([cte, psi], ord=float(self._norm_ord))
+
+        def spec(x, y, theta, cte, psi) -> bool:
+            v_old = v(*m_star(x, y, theta))
+            v_new = v(*m_star(*f(x, y, theta, *g(cte, psi))))
+            return v_new <= max(v_old, self._ultimate_bound)
         return spec(*ex)
 
     def _add_system(self) -> None:
@@ -144,7 +150,7 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
         m = self._gp_model
         old_x, old_y, old_yaw = self._old_state.tolist()
         new_x, new_y, new_yaw = self._new_state.tolist()
-        cte, phi = self._percept.tolist()
+        cte, psi = self._percept.tolist()
         steering, = self._control.tolist()
 
         # Controller
@@ -156,7 +162,7 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
         # Clip steering angle
         error = m.addVar(name="(φ+atan(K*d/Vf))",
                          lb=-RAW_ANG_ERR_LIM, ub=RAW_ANG_ERR_LIM)
-        m.addConstr(error == phi + atan_K_cte_V)
+        m.addConstr(error == psi + atan_K_cte_V)
 
         xpts = [-RAW_ANG_ERR_LIM, -STEERING_LIM, STEERING_LIM, RAW_ANG_ERR_LIM]
         ypts = [-STEERING_LIM, -STEERING_LIM, STEERING_LIM, STEERING_LIM]
@@ -194,6 +200,16 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
                                lb=0.0, ub=RAW_ANG_ERR_LIM)
         m.addConstr(old_lya_val == gp.norm(old_truth, float(self._norm_ord)))
 
+        # Add ultimate bound
+        if self._ultimate_bound > 0:
+            bound_new_lya_val = m.addVar(name=f"max(|m(x)|,{self._ultimate_bound})",
+                                         vtype=gp.GRB.CONTINUOUS,
+                               lb=0.0, ub=RAW_ANG_ERR_LIM)
+            m.addConstr(bound_new_lya_val == gp.max_(old_lya_val, constant=self._ultimate_bound))
+        else:
+            bound_new_lya_val = old_lya_val
+
+
         new_truth = m.addMVar(shape=(self.perc_dim,), name="m(x')", **self.FREEVAR)
         m.addConstr(new_truth == self.PERC_GT @ self._new_state)
         new_lya_val = m.addVar(name="|m(x')|", vtype=gp.GRB.CONTINUOUS,
@@ -249,10 +265,10 @@ class GEMStanleySymPyTeacher(SymPyTeacherBase):
         # Variable Aliases
         old_x, old_y, old_yaw = self._old_state
         new_x, new_y, new_yaw = self._new_state
-        cte, phi = self._percept
+        cte, psi = self._percept
         steering, = self._control
 
-        err = phi + sympy.atan2(cte*self.K_P, self.FORWARD_VEL)
+        err = psi + sympy.atan2(cte*self.K_P, self.FORWARD_VEL)
         clipped_err = sympy.Piecewise(
             (self.STEERING_LIM, err > self.STEERING_LIM),
             (-self.STEERING_LIM, err < -self.STEERING_LIM),
@@ -313,7 +329,7 @@ def test_gem_stanley_sympy_teacher():
 
 
 def test_gem_stanley_gurobi_teacher():
-    teacher = GEMStanleyGurobiTeacher()
+    teacher = GEMStanleyGurobiTeacher(ultimate_bound=.4)
     teacher.set_old_state_bound(
         lb=(-np.inf, 0.5, 0.0625),
         ub=(np.inf, 1.0, 0.25)