From a2848bbf74b033604fd7ea759c117232b08ecd66 Mon Sep 17 00:00:00 2001
From: Angello Astorga <aastorg2@illinois.edu>
Date: Tue, 25 Oct 2022 22:59:28 -0500
Subject: [PATCH] working agbot learner
---
agbot_stanley_teacher.py | 14 +++++++-------
1 file changed, 7 insertions(+), 7 deletions(-)
diff --git a/agbot_stanley_teacher.py b/agbot_stanley_teacher.py
index ce735be..9e3b454 100644
--- a/agbot_stanley_teacher.py
+++ b/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)
--
GitLab