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