Specify velocity as a parameter

gem_stanley_teacher.py

@@ -110,11 +110,9 @@ class DTreeGEMStanleyGurobiStabilityTeacher(DTreeGEMStanleyGurobiTeacherBase):
         self._ultimate_bound = ultimate_bound
         super().__init__(name=name, norm_ord=norm_ord)
 
-    def is_safe_state(self, ex) -> bool:
+    def is_safe_state(self, ex, vel=FORWARD_VEL_MAX) -> bool:
         assert len(ex) == self.state_dim + self.perc_dim
 
-        vel = FORWARD_VEL_MIN
-
         def g(perc):
             cte, psi = perc
             error = psi + np.arctan(K_P*cte/vel)
@@ -179,13 +177,12 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
 
         super().__init__(name=name, norm_ord=norm_ord)
 
-    def is_safe_state(self, ex) -> bool:
+    def is_safe_state(self, ex, vel=FORWARD_VEL_MAX) -> bool:
         assert len(ex) == self.state_dim + self.perc_dim
 
-        vel = FORWARD_VEL_MIN
         def g(perc):
             cte, psi = perc
-            error = psi + np.arctan(K_P*cte/FORWARD_VEL)
+            error = psi + np.arctan(K_P*cte/vel)
             steer = np.clip(error, -STEERING_LIM, STEERING_LIM)
             return (steer,)