From 85362be90e47d95ba2e7082818918ea886316ce4 Mon Sep 17 00:00:00 2001
From: "Hsieh, Chiao" <chsieh16@illinois.edu>
Date: Thu, 27 Oct 2022 20:43:13 -0500
Subject: [PATCH] Rename ctd back to cte fpr consistency
---
agbot_stanley_teacher.py | 20 ++++++++++----------
gem_stanley_teacher.py | 12 ++++++------
2 files changed, 16 insertions(+), 16 deletions(-)
diff --git a/agbot_stanley_teacher.py b/agbot_stanley_teacher.py
index 21cccd3..e1e3fbf 100644
--- a/agbot_stanley_teacher.py
+++ b/agbot_stanley_teacher.py
@@ -55,8 +55,8 @@ class DTreeAgBotStanleyGurobiTeacher(DTreeGurobiTeacherBase):
return new_x, new_y, new_yaw
def g(perc):
- prcv_ctd, prcv_heading = perc
- error = prcv_heading + np.arctan(K_P*prcv_ctd/FORWARD_VEL)
+ prcv_cte, prcv_heading = perc
+ error = prcv_heading + np.arctan(K_P*prcv_cte/FORWARD_VEL)
# Calculate controller output
ang_vel = error / CYCLE_TIME
if ang_vel > ANG_VEL_LIM:
@@ -72,8 +72,8 @@ class DTreeAgBotStanleyGurobiTeacher(DTreeGurobiTeacherBase):
return self.PERC_GT @ state
def v(gt_perc) -> float:
- ctd, psi = gt_perc
- return abs(psi + np.arctan((K_P*ctd) / FORWARD_VEL))
+ cte, psi = gt_perc
+ return abs(psi + np.arctan((K_P*cte) / FORWARD_VEL))
def spec(state, perc) -> bool:
v_old = v(m_star(state))
@@ -95,20 +95,20 @@ class DTreeAgBotStanleyGurobiTeacher(DTreeGurobiTeacherBase):
m = self._gp_model
old_x, old_y, old_yaw = self._old_state.tolist()
new_x, new_y, new_yaw = self._new_state.tolist()
- ctd, psi = self._percept.tolist()
+ cte, psi = self._percept.tolist()
ang_vel, = self._control.tolist()
# Controller
- K_ctd_V = m.addVar(name="K*dhat/Vf", lb=-K_CTE_V_LIM, ub=K_CTE_V_LIM)
- m.addConstr(K_ctd_V == K_P*ctd/FORWARD_VEL)
- atan_K_ctd_V = m.addVar(name="atan(K*dhat/Vf)",
+ K_cte_V = m.addVar(name="K*dhat/Vf", lb=-K_CTE_V_LIM, ub=K_CTE_V_LIM)
+ m.addConstr(K_cte_V == K_P*cte/FORWARD_VEL)
+ atan_K_cte_V = m.addVar(name="atan(K*dhat/Vf)",
lb=-ATAN_K_CTE_V_LIM, ub=ATAN_K_CTE_V_LIM)
- m.addGenConstrTan(xvar=atan_K_ctd_V, yvar=K_ctd_V)
+ m.addGenConstrTan(xvar=atan_K_cte_V, yvar=K_cte_V)
# Clip angular velocity
raw_ang_vel = m.addVar(name="(φhat+atan(K*dhat/V))/T",
lb=-RAW_ANG_VEL_LIM, ub=RAW_ANG_VEL_LIM)
- m.addConstr(raw_ang_vel == (psi + atan_K_ctd_V) / CYCLE_TIME)
+ m.addConstr(raw_ang_vel == (psi + atan_K_cte_V) / CYCLE_TIME)
ang_vel = m.addVar(name="ω", lb=-ANG_VEL_LIM, ub=ANG_VEL_LIM)
m.addGenConstrPWL(name="clip", xvar=raw_ang_vel, yvar=ang_vel,
diff --git a/gem_stanley_teacher.py b/gem_stanley_teacher.py
index fb5ec02..402204a 100644
--- a/gem_stanley_teacher.py
+++ b/gem_stanley_teacher.py
@@ -46,8 +46,8 @@ class DTreeGEMStanleyGurobiTeacher(DTreeGurobiTeacherBase):
assert len(ex) == self.state_dim + self.perc_dim
def g(perc):
- ctd, psi = perc
- error = psi + np.arctan(K_P*ctd/FORWARD_VEL)
+ cte, psi = perc
+ error = psi + np.arctan(K_P*cte/FORWARD_VEL)
steer = np.clip(error, -STEERING_LIM, STEERING_LIM)
return (steer,)
@@ -85,12 +85,12 @@ class DTreeGEMStanleyGurobiTeacher(DTreeGurobiTeacherBase):
m = self._gp_model
old_x, old_y, old_yaw = self._old_state.tolist()
new_x, new_y, new_yaw = self._new_state.tolist()
- ctd, psi = self._percept.tolist()
+ cte, psi = self._percept.tolist()
steering, = self._control.tolist()
# Controller
K_cte_V = m.addVar(name="K*d/Vf", lb=-K_CTE_V_LIM, ub=K_CTE_V_LIM)
- m.addConstr(K_cte_V == K_P*ctd/FORWARD_VEL)
+ m.addConstr(K_cte_V == K_P*cte/FORWARD_VEL)
atan_K_cte_V = m.addVar(name="atan(K*d/Vf)",
lb=-ATAN_K_CTE_V_LIM, ub=ATAN_K_CTE_V_LIM)
m.addGenConstrTan(xvar=atan_K_cte_V, yvar=K_cte_V)
@@ -197,10 +197,10 @@ class GEMStanleySymPyTeacher(SymPyTeacherBase):
# Variable Aliases
old_x, old_y, old_yaw = self._old_state
new_x, new_y, new_yaw = self._new_state
- ctd, psi = self._percept
+ cte, psi = self._percept
steering, = self._control
- err = psi + sympy.atan2(ctd*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),
--
GitLab