working agbot learner

agbot_stanley_teacher.py

@@ -32,15 +32,15 @@ def sensor(state):
         Hence, heading = 0-yaw = -yaw and distance = 0-y = -y."""
     x, y, yaw = state
     # TODO approximation instead of perfect perception
+    # TODO is this where we do AX+b
     prcv_heading = -yaw
     prcv_distance = -y
-    return prcv_heading, prcv_distance
+    return prcv_distance,prcv_heading
 
 
-def controller(epsilon):
-    prcv_heading, prcv_distance = epsilon
-    error = prcv_heading + np.arctan2(K_P*prcv_distance, FORWARD_VEL)
-
+def controller(prcv_ctd, prcv_heading):
+    error = prcv_heading + np.arctan(K_P*prcv_ctd/FORWARD_VEL)
+    
     # Calculate controller output
     ang_vel = error / CYCLE_TIME
     if ang_vel > ANG_VEL_LIM:
@@ -78,7 +78,7 @@ class DTreeAgBotStanleyGurobiTeacher(DTreeGurobiTeacherBase):
         super().__init__(name=name,
                          state_dim=3, perc_dim=2, ctrl_dim=1, norm_ord=norm_ord)
 
-    def is_positive_example(self, ex) -> bool:
+    def is_safe_state(self, ex) -> bool:
         def v(ctd, psi) -> float:
             return abs(psi + np.arctan( (K_P*ctd) / FORWARD_VEL))
 
@@ -87,7 +87,7 @@ class DTreeAgBotStanleyGurobiTeacher(DTreeGurobiTeacherBase):
             f = dynamics
             g = controller
             v_old = v(*m_star((x, y, theta)))
-            v_new = v(*m_star( f((x, y, theta),*g((ctd, psi))) ))
+            v_new = v(*m_star( f((x, y, theta),*g(ctd, psi)) ))
             return v_new <= max(v_old, self._ultimate_bound)
         return spec(*ex)