Ask teacher to check positive examples

gem_stanley_teacher.py

@@ -87,6 +87,29 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
         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)
+            steer = np.clip(error, -STEERING_LIM, STEERING_LIM)
+            return (steer,)
+
+        def f(x, y, theta, steer):
+            new_x = x + FORWARD_VEL*np.cos(theta+steer)*CYCLE_TIME
+            new_y = y + FORWARD_VEL*np.sin(theta+steer)*CYCLE_TIME
+            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 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
+        return spec(*ex)
+
     def _add_system(self) -> None:
         # Bounds on all domains
         self._old_state.lb = (-np.inf, -CTE_LIM, -ANG_LIM)

teacher_base.py

@@ -13,6 +13,10 @@ class TeacherBase(abc.ABC):
     def __init__(self) -> None:
         pass
 
+    @abc.abstractmethod
+    def is_positive_example(self, ex) -> bool:
+        raise NotImplementedError
+
     @abc.abstractmethod
     def check(self, candidate: sympy.logic.boolalg.Boolean) -> z3.CheckSatResult:
         raise NotImplementedError