From fad36cab2ab566951e6718dd9685f2114ad319b8 Mon Sep 17 00:00:00 2001
From: "Hsieh, Chiao" <chsieh16@illinois.edu>
Date: Sun, 16 Oct 2022 22:55:40 -0500
Subject: [PATCH] Add AgBotStanley teacher (not tested)
---
agbot_stanley_teacher.py | 213 +++++++++++++++++++++++++++++++++++++++
1 file changed, 213 insertions(+)
create mode 100644 agbot_stanley_teacher.py
diff --git a/agbot_stanley_teacher.py b/agbot_stanley_teacher.py
new file mode 100644
index 0000000..4343f9a
--- /dev/null
+++ b/agbot_stanley_teacher.py
@@ -0,0 +1,213 @@
+import gurobipy as gp
+import numpy as np
+
+from dtree_teacher_base import DTreeGurobiTeacherBase
+
+
+K_P = 0.1
+FORWARD_VEL = 1.0 # m/s
+CYCLE_TIME = 0.05 # s
+
+ANG_VEL_LIM = 0.5 # rad/s^2
+
+# Limits on unconstrained variables to avoid overflow and angle normalization
+ANG_LIM = np.pi / 2 # radian
+CTE_LIM = 0.76 * 0.5 # meter
+
+
+# Bounds for intermediate variables.
+# These are not necessary but can improve efficiency
+K_CTE_V_LIM = K_P * CTE_LIM / FORWARD_VEL
+ATAN_K_CTE_V_LIM = np.arctan(K_CTE_V_LIM)
+RAW_ANG_ERR_LIM = (ANG_LIM + ATAN_K_CTE_V_LIM)
+RAW_ANG_VEL_LIM = RAW_ANG_ERR_LIM / CYCLE_TIME
+
+NEW_K_CTE_V_LIM = K_CTE_V_LIM + FORWARD_VEL * CYCLE_TIME * 1.0
+NEW_ATAN_K_CTE_V_LIM = np.arctan(NEW_K_CTE_V_LIM)
+NEW_RAW_ANG_ERR_LIM = ANG_LIM + FORWARD_VEL * CYCLE_TIME
+
+
+def sensor(state):
+ """ Assuming the lane to track is aligned with x-axis (i.e., y==0 and yaw==0)
+ Hence, heading = 0-yaw = -yaw and distance = 0-y = -y."""
+ x, y, yaw = state
+ # TODO approximation instead of perfect perception
+ prcv_heading = -yaw
+ prcv_distance = -y
+ return prcv_heading, prcv_distance
+
+
+def controller(epsilon):
+ prcv_heading, prcv_distance = epsilon
+ error = prcv_heading + np.arctan2(K_P*prcv_distance, FORWARD_VEL)
+
+ # Calculate controller output
+ ang_vel = error / CYCLE_TIME
+ if ang_vel > ANG_VEL_LIM:
+ ang_vel = ANG_VEL_LIM
+ elif ang_vel < -ANG_VEL_LIM:
+ ang_vel = -ANG_VEL_LIM
+
+ # Return actuator values
+ return (ang_vel,)
+
+
+def dynamics(old_state, ang_vel):
+ """ This dynamics for state variables x, y, yaw
+
+ x[n+1] = x[n] + v*cos(yaw)*CYCLE_TIME
+ y[n+1] = y[n] + v*sin(yaw)*CYCLE_TIME
+ yaw[n+1] = yaw[n] + ang_vel*CYCLE_TIME
+ """
+ old_x, old_y, old_yaw = old_state
+ new_x = old_x + FORWARD_VEL * np.cos(old_yaw) * CYCLE_TIME
+ new_y = old_y + FORWARD_VEL * np.sin(old_yaw) * CYCLE_TIME
+ new_yaw = old_yaw + ang_vel*CYCLE_TIME
+ return new_x, new_y, new_yaw
+
+
+class DTreeAgBotStanleyGurobiTeacher(DTreeGurobiTeacherBase):
+ # Ideal perception as a linear transform from state to ground truth percept
+ PERC_GT = np.array([[0., -1., 0.],
+ [0., 0., -1.]], float)
+
+ def __init__(self, name="agbot_stanley", norm_ord=2, ultimate_bound: float=0.0) -> None:
+ assert ultimate_bound >= 0.0
+ self._ultimate_bound = ultimate_bound
+
+ 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 v(cte, psi) -> float:
+ return np.linalg.norm([cte, psi], ord=float(self._norm_ord))
+
+ def spec(x, y, theta, cte, psi) -> bool:
+ m_star = sensor
+ f = dynamics
+ g = controller
+ v_old = v(*m_star(x, y, theta))
+ v_new = v(*m_star(*f(x, y, theta, *g(cte, psi))))
+ return v_new <= max(v_old, self._ultimate_bound)
+ return spec(*ex)
+
+ def _add_system(self) -> None:
+ # Bounds on all domains
+ self._old_state.lb = (-np.inf, -CTE_LIM, -ANG_LIM)
+ self._old_state.ub = (np.inf, CTE_LIM, ANG_LIM)
+ self._percept.lb = (-CTE_LIM, -ANG_LIM)
+ self._percept.ub = (CTE_LIM, ANG_LIM)
+ self._control.lb = (-ANG_VEL_LIM,)
+ self._control.ub = (ANG_VEL_LIM,)
+
+ # Variable Aliases
+ m = self._gp_model
+ old_x, old_y, old_yaw = self._old_state.tolist()
+ new_x, new_y, new_yaw = self._new_state.tolist()
+ cte, psi = self._percept.tolist()
+ ang_vel, = self._control.tolist()
+
+ # Controller
+ 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_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_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,
+ xpts=[-RAW_ANG_VEL_LIM, -ANG_VEL_LIM, ANG_VEL_LIM, RAW_ANG_VEL_LIM],
+ ypts=[-ANG_VEL_LIM, -ANG_VEL_LIM, ANG_VEL_LIM, ANG_VEL_LIM])
+
+ # Dynamics
+ cos_yaw = m.addVar(name="cosθ", **self.TRIGVAR)
+ m.addGenConstrCos(old_yaw, cos_yaw)
+ sin_yaw = m.addVar(name="sinθ", **self.TRIGVAR)
+ m.addGenConstrSin(old_yaw, sin_yaw)
+
+ new_x = old_x + FORWARD_VEL * CYCLE_TIME * cos_yaw
+ new_y = m.addVar(name="y'", **self.FREEVAR)
+ m.addConstr(new_y == old_y + FORWARD_VEL * CYCLE_TIME * sin_yaw)
+ new_yaw = m.addVar(name="θ'", **self.FREEVAR)
+ m.addConstr(new_yaw == old_yaw + ang_vel * CYCLE_TIME)
+ m.update()
+
+ def _add_unsafe(self) -> None:
+ assert self.PERC_GT.shape == (self.perc_dim, self.state_dim)
+ # Variable Aliases
+ m = self._gp_model
+
+ # Add nonincreasing constraints
+ old_truth = m.addMVar(shape=(self.perc_dim,), name="m(x)", **self.FREEVAR)
+ m.addConstr(old_truth == self.PERC_GT @ self._old_state)
+
+ old_cte, old_psi = old_truth.tolist()
+ K_old_cte_V = m.addVar(name="K*d/Vf", lb=-K_CTE_V_LIM, ub=K_CTE_V_LIM)
+ m.addConstr(K_old_cte_V == K_P * old_cte / FORWARD_VEL)
+ atan_K_old_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_old_cte_V, yvar=K_old_cte_V)
+ old_err = m.addVar(name="ψ+atan(K*d/Vf)", **self.FREEVAR)
+ m.addConstr(old_err == old_psi + atan_K_old_cte_V)
+
+ old_lya_val = m.addVar(name="V(m(x))", **self.NNEGVAR)
+ m.addConstr(old_lya_val == gp.abs_(old_err))
+
+ # TODO What is the Lyapunov function?
+ # Add ultimate bound
+ if self._ultimate_bound > 0:
+ bound_new_lya_val = m.addVar(name=f"max(|m(x)|,{self._ultimate_bound})",
+ **self.NNEGVAR)
+ m.addConstr(bound_new_lya_val == gp.max_(old_lya_val, constant=self._ultimate_bound))
+ else:
+ bound_new_lya_val = old_lya_val
+
+ new_truth = m.addMVar(shape=(self.perc_dim,), name="m(x')", **self.FREEVAR)
+ m.addConstr(new_truth == self.PERC_GT @ self._new_state)
+
+ new_cte, new_psi = new_truth.tolist()
+ K_new_cte_V = m.addVar(name="K*d'/Vf", lb=-K_CTE_V_LIM, ub=K_CTE_V_LIM)
+ m.addConstr(K_new_cte_V == K_P * new_cte / FORWARD_VEL)
+ atan_K_new_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_new_cte_V, yvar=K_new_cte_V)
+ new_err = m.addVar(name="ψ'+atan(K*d'/Vf)", **self.FREEVAR)
+ m.addConstr(new_err == new_psi + atan_K_new_cte_V)
+ new_lya_val = m.addVar(name="V(m(x'))", **self.NNEGVAR)
+ m.addConstr(new_lya_val == gp.abs_(new_err))
+
+ # Negation of stability spec
+ m.addConstr(new_lya_val >= bound_new_lya_val, name="Increasing Error") # Tracking error is increasing
+ m.update()
+
+
+ def _add_objective(self) -> None:
+ # Variable Aliases
+ m = self._gp_model
+ x = self._old_state
+ z = self._percept
+ z_diff = self._percept_diff
+
+ # Add objective
+ norm_var = m.addVar(name="|z-m(x)|", **self.NNEGVAR)
+ m.addConstr(z_diff == z - self.PERC_GT @ x)
+ m.addConstr(norm_var == gp.norm(z_diff, float(self._norm_ord)))
+
+ m.setObjective(norm_var, gp.GRB.MINIMIZE)
+ m.update()
+
+
+def test_gem_stanley_gurobi_teacher():
+ teacher = DTreeAgBotStanleyGurobiTeacher(ultimate_bound=.0)
+ teacher.set_old_state_bound(
+ lb=(-np.inf, 0.5, 0.0625),
+ ub=(np.inf, 1.0, 0.25)
+ )
+ teacher.dump_system_encoding()
+
+
+if __name__ == "__main__":
+ test_gem_stanley_gurobi_teacher()
--
GitLab