Change constant forward velocity to a range

gem_stanley_teacher.py

@@ -11,7 +11,8 @@ WHEEL_BASE = 1.75  # m
 K_P = 0.45
 
 CYCLE_TIME = 0.05  # second
-FORWARD_VEL = 2.8  # m/s
+FORWARD_VEL_MIN = 2.5  # m/s
+FORWARD_VEL_MAX = 3.0  # m/s
 STEERING_LIM = 0.61  # rad
 
 # Limits on unconstrained variables to avoid overflow and angle normalization
@@ -20,14 +21,10 @@ CTE_LIM = 2.0  # meter
 
 # Bounds for intermediate variables.
 # These are not necessary but can improve efficiency
-K_CTE_V_LIM = K_P * CTE_LIM / FORWARD_VEL
+K_CTE_V_LIM = K_P * 20 * CTE_LIM / FORWARD_VEL_MIN
 ATAN_K_CTE_V_LIM = np.arctan(K_CTE_V_LIM)
 RAW_ANG_ERR_LIM = (ANG_LIM + ATAN_K_CTE_V_LIM)
 
-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
-
 
 class DTreeGEMStanleyGurobiTeacherBase(DTreeGurobiTeacherBase):
     # Ideal perception as a linear transform from state to ground truth percept
@@ -54,9 +51,11 @@ class DTreeGEMStanleyGurobiTeacherBase(DTreeGurobiTeacherBase):
         cte, psi = self._percept.tolist()
         steering, = self._control.tolist()
 
+        vel = m.addVar(name="Vf", lb=FORWARD_VEL_MIN, ub=FORWARD_VEL_MAX)
+
         # 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*cte/FORWARD_VEL)
+        m.addConstr(cte == K_cte_V*vel/K_P)
         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)
@@ -83,10 +82,10 @@ class DTreeGEMStanleyGurobiTeacherBase(DTreeGurobiTeacherBase):
         sin_steer = m.addVar(name="sinδ", **self.TRIGVAR)
         m.addGenConstrSin(steering, sin_steer)
 
-        m.addConstr(new_x == old_x + FORWARD_VEL*CYCLE_TIME*cos_yaw_steer)
-        m.addConstr(new_y == old_y + FORWARD_VEL*CYCLE_TIME*sin_yaw_steer)
+        m.addConstr(new_x == old_x + vel*CYCLE_TIME*cos_yaw_steer)
+        m.addConstr(new_y == old_y + vel*CYCLE_TIME*sin_yaw_steer)
         m.addConstr(new_yaw ==
-                    old_yaw + sin_steer*FORWARD_VEL*CYCLE_TIME/WHEEL_BASE)
+                    old_yaw + sin_steer*vel*CYCLE_TIME/WHEEL_BASE)
         m.update()
 
     def _add_objective(self) -> None:
@@ -114,18 +113,20 @@ class DTreeGEMStanleyGurobiStabilityTeacher(DTreeGEMStanleyGurobiTeacherBase):
     def is_safe_state(self, ex) -> bool:
         assert len(ex) == self.state_dim + self.perc_dim
 
+        vel = FORWARD_VEL_MIN
+
         def g(perc):
             cte, psi = perc
-            error = psi + np.arctan(K_P*cte/FORWARD_VEL)
+            error = psi + np.arctan(K_P*cte/vel)
             steer = np.clip(error, -STEERING_LIM, STEERING_LIM)
             return (steer,)
 
         def f(state, ctrl):
             x, y, theta = state
             steer, = ctrl
-            new_x = x + FORWARD_VEL*np.cos(theta+steer)*CYCLE_TIME
-            new_y = y + FORWARD_VEL*np.sin(theta+steer)*CYCLE_TIME
-            new_theta = theta + FORWARD_VEL*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
+            new_x = x + vel*np.cos(theta+steer)*CYCLE_TIME
+            new_y = y + vel*np.sin(theta+steer)*CYCLE_TIME
+            new_theta = theta + vel*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
             return new_x, new_y, new_theta
 
         def m_star(state):
@@ -181,6 +182,7 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
     def is_safe_state(self, ex) -> bool:
         assert len(ex) == self.state_dim + self.perc_dim
 
+        vel = FORWARD_VEL_MIN
         def g(perc):
             cte, psi = perc
             error = psi + np.arctan(K_P*cte/FORWARD_VEL)
@@ -190,9 +192,9 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
         def f(state, ctrl):
             x, y, theta = state
             steer, = ctrl
-            new_x = x + FORWARD_VEL*np.cos(theta+steer)*CYCLE_TIME
-            new_y = y + FORWARD_VEL*np.sin(theta+steer)*CYCLE_TIME
-            new_theta = theta + FORWARD_VEL*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
+            new_x = x + vel*np.cos(theta+steer)*CYCLE_TIME
+            new_y = y + vel*np.sin(theta+steer)*CYCLE_TIME
+            new_theta = theta + vel*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
             return new_x, new_y, new_theta
 
         def m_star(state):