Add AgBotStanley teacher (not tested)

agbot_stanley_teacher.py

+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()