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): ...@@ -110,11 +110,9 @@ class DTreeGEMStanleyGurobiStabilityTeacher(DTreeGEMStanleyGurobiTeacherBase):
self._ultimate_bound = ultimate_bound self._ultimate_bound = ultimate_bound
super().__init__(name=name, norm_ord=norm_ord) 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 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/vel) error = psi + np.arctan(K_P*cte/vel)
...@@ -179,13 +177,12 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase): ...@@ -179,13 +177,12 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
super().__init__(name=name, norm_ord=norm_ord) 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 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,)
......
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