Skip to content
Snippets Groups Projects
Commit f502cc00 authored by chsieh16's avatar chsieh16
Browse files

Change constant forward velocity to a range

parent 99ba772f
No related branches found
No related tags found
No related merge requests found
......@@ -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):
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment