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

Specify velocity as a parameter

parent 4e2b8f07
No related branches found
No related tags found
No related merge requests found
......@@ -110,11 +110,9 @@ class DTreeGEMStanleyGurobiStabilityTeacher(DTreeGEMStanleyGurobiTeacherBase):
self._ultimate_bound = ultimate_bound
super().__init__(name=name, norm_ord=norm_ord)
def is_safe_state(self, ex) -> bool:
def is_safe_state(self, ex, vel=FORWARD_VEL_MAX) -> 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/vel)
......@@ -179,13 +177,12 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
super().__init__(name=name, norm_ord=norm_ord)
def is_safe_state(self, ex) -> bool:
def is_safe_state(self, ex, vel=FORWARD_VEL_MAX) -> 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,)
......
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