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

Add sympy teacher

parent 0f3cc5a1
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 110 additions and 19 deletions
gem_stanley_teacher.py
+ 110
− 19
View file @ ba4170fd
......@@ -3,8 +3,9 @@ import dreal
import gurobipy as gp
from gurobipy import GRB
import numpy as np
import sympy
from teacher_base import GurobiTeacherBase, DRealTeacherBase
from teacher_base import GurobiTeacherBase, DRealTeacherBase, SymPyTeacherBase
WHEEL_BASE = 1.75 # m
......@@ -29,6 +30,54 @@ 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 GEMStanleyDRealTeacher(DRealTeacherBase):
def __init__(self, name="gem_stanley", norm_ord=2, delta=0.001) -> None:
super().__init__(name=name,
state_dim=3, perc_dim=2, ctrl_dim=1, norm_ord=norm_ord, delta=delta)
def _add_system(self) -> None:
self._set_var_bound(self._old_state, lb=(-np.inf, -CTE_LIM, -ANG_LIM), ub=(np.inf, CTE_LIM, ANG_LIM))
self._set_var_bound(self._new_state, lb=(-np.inf, -CTE_LIM, -ANG_LIM), ub=(np.inf, CTE_LIM, ANG_LIM))
self._set_var_bound(self._percept, lb=(-CTE_LIM, -ANG_LIM), ub=(CTE_LIM, ANG_LIM))
self._set_var_bound(self._control, lb=(-STEERING_LIM,), ub=(STEERING_LIM,))
# Variable Aliases
old_x, old_y, old_yaw = self._old_state
new_x, new_y, new_yaw = self._new_state
cte, phi = self._percept
steering, = self._control
self._not_inv_cons.extend([
# Control
steering == dreal.Min(dreal.Max(phi + dreal.atan(cte*(K_P / FORWARD_VEL)), -STEERING_LIM), STEERING_LIM),
# Dynamics
new_x == old_x + FORWARD_VEL*CYCLE_TIME*dreal.cos(old_yaw + steering),
new_y == old_y + FORWARD_VEL*CYCLE_TIME*dreal.sin(old_yaw + steering),
new_yaw == old_yaw + dreal.sin(steering)*FORWARD_VEL*CYCLE_TIME/WHEEL_BASE,
])
def _add_unsafe(self) -> None:
# Variable Aliases
old_x, old_y, old_yaw = self._old_state
new_x, new_y, new_yaw = self._new_state
if self._norm_ord == 1:
old_err = abs(old_y) + abs(old_yaw)
new_err = abs(new_y) + abs(new_yaw)
elif self._norm_ord == 2:
old_err = dreal.sqrt(old_y**2 + old_yaw**2)
new_err = dreal.sqrt(new_y**2 + new_yaw**2)
else:
assert self._norm_ord == "inf"
old_err = dreal.Max(abs(old_y), abs(old_yaw))
new_err = dreal.Max(abs(new_y), abs(new_yaw))
self._not_inv_cons.extend([
old_err <= new_err
])
class GEMStanleyGurobiTeacher(GurobiTeacherBase):
def __init__(self, name="gem_stanley") -> None:
super().__init__(name=name,
......@@ -122,16 +171,32 @@ class GEMStanleyGurobiTeacher(GurobiTeacherBase):
m.addConstr(new_V >= old_V) # Tracking error is increasing (UNSAFE)
class GEMStanleyDRealTeacher(DRealTeacherBase):
class GEMStanleySymPyTeacher(SymPyTeacherBase):
CTE_LIM = sympy.Rational("2.0")
ANG_LIM = sympy.pi / 2
STEERING_LIM = sympy.Rational("0.61")
K_P = sympy.Rational("0.45")
FORWARD_VEL = sympy.Rational("2.8")
CYCLE_TIME = sympy.Rational("0.05")
WHEEL_BASE = sympy.Rational("1.75")
def __init__(self, name="gem_stanley", norm_ord=2, delta=0.001) -> None:
def __init__(self, name="gem_stanley", norm_ord=2) -> None:
super().__init__(name=name,
state_dim=3, perc_dim=2, ctrl_dim=1, norm_ord=norm_ord, delta=delta)
state_dim=3, perc_dim=2, ctrl_dim=1, norm_ord=norm_ord)
def _add_system(self) -> None:
self._set_var_bound(self._old_state, lb=(-np.inf, -CTE_LIM, -ANG_LIM), ub=(np.inf, CTE_LIM, ANG_LIM))
self._set_var_bound(self._percept, lb=(-CTE_LIM, -ANG_LIM), ub=(CTE_LIM, ANG_LIM))
self._set_var_bound(self._control, lb=(-STEERING_LIM,), ub=(STEERING_LIM,))
self._set_var_bound(self._old_state,
lb=(-sympy.oo, -self.CTE_LIM, -self.ANG_LIM),
ub=(sympy.oo, self.CTE_LIM, self.ANG_LIM))
self._set_var_bound(self._new_state,
lb=(-sympy.oo, -self.CTE_LIM, -self.ANG_LIM),
ub=(sympy.oo, self.CTE_LIM, self.ANG_LIM))
self._set_var_bound(self._percept,
lb=(-self.CTE_LIM, -self.ANG_LIM),
ub=(self.CTE_LIM, self.ANG_LIM))
self._set_var_bound(self._control,
lb=(-self.STEERING_LIM,),
ub=(self.STEERING_LIM,))
# Variable Aliases
old_x, old_y, old_yaw = self._old_state
......@@ -139,13 +204,20 @@ class GEMStanleyDRealTeacher(DRealTeacherBase):
cte, phi = self._percept
steering, = self._control
err = phi + sympy.atan2(cte*self.K_P, self.FORWARD_VEL)
clipped_err = sympy.Piecewise(
(self.STEERING_LIM, err > self.STEERING_LIM),
(-self.STEERING_LIM, err < -self.STEERING_LIM),
(err, True)
)
self._not_inv_cons.extend([
# Control
steering == dreal.Min(dreal.Max(phi + dreal.atan(cte*(K_P / FORWARD_VEL)), -STEERING_LIM), STEERING_LIM),
sympy.Eq(steering, clipped_err),
# Dynamics
new_x == old_x + FORWARD_VEL*CYCLE_TIME*dreal.cos(old_yaw + steering),
new_y == old_y + FORWARD_VEL*CYCLE_TIME*dreal.sin(old_yaw + steering),
new_yaw == old_yaw + dreal.sin(steering)*FORWARD_VEL*CYCLE_TIME/WHEEL_BASE,
sympy.Eq(new_x, old_x + self.FORWARD_VEL*self.CYCLE_TIME*sympy.cos(old_yaw + steering)),
sympy.Eq(new_y, old_y + self.FORWARD_VEL*self.CYCLE_TIME*sympy.sin(old_yaw + steering)),
sympy.Eq(new_yaw, old_yaw + sympy.sin(steering)*self.FORWARD_VEL*self.CYCLE_TIME/self.WHEEL_BASE),
])
def _add_unsafe(self) -> None:
......@@ -154,22 +226,22 @@ class GEMStanleyDRealTeacher(DRealTeacherBase):
new_x, new_y, new_yaw = self._new_state
if self._norm_ord == 1:
old_err = abs(old_y) + abs(old_yaw)
new_err = abs(new_y) + abs(new_yaw)
old_err = sympy.Abs(old_y) + sympy.Abs(old_yaw)
new_err = sympy.Abs(new_y) + sympy.Abs(new_yaw)
elif self._norm_ord == 2:
old_err = dreal.sqrt(old_y**2 + old_yaw**2)
new_err = dreal.sqrt(new_y**2 + new_yaw**2)
old_err = sympy.sqrt(old_y**2 + old_yaw**2)
new_err = sympy.sqrt(new_y**2 + new_yaw**2)
else:
assert self._norm_ord == "inf"
old_err = dreal.Max(abs(old_y), abs(old_yaw))
new_err = dreal.Max(abs(new_y), abs(new_yaw))
old_err = sympy.Max(sympy.Abs(old_y), sympy.Abs(old_yaw))
new_err = sympy.Max(sympy.Abs(new_y), sympy.Abs(new_yaw))
self._not_inv_cons.extend([
old_err <= new_err
])
def test_gem_stanley_teacher():
def test_gem_stanley_dreal_teacher():
a_mat = np.asfarray([[0, -1, 0],
[0, 0, -1]])
b_vec = np.zeros(2)
......@@ -189,8 +261,27 @@ def test_gem_stanley_teacher():
# teacher.dump_model()
res = teacher.check((a_mat, b_vec, coeff_mat, cut_vec))
print(res)
def test_gem_stanley_sympy_teacher():
teacher = GEMStanleySymPyTeacher(norm_ord=2)
teacher.set_old_state_bound(
lb=(-sympy.oo, sympy.Rational("0.5"), 0.0),
ub=(sympy.oo, sympy.Rational("1.2"), sympy.pi/12)
)
teacher.dump_system_formula()
def test_gem_stanley_gurobi_teacher():
teacher = GEMStanleyGurobiTeacher()
teacher.set_old_state_bound(
lb=(-np.inf, 0.5, 0.0625),
ub=(np.inf, 1.0, 0.25)
)
teacher.dump_model()
print(teacher.check(None))
print(teacher.model())
if __name__ == "__main__":
test_gem_stanley_teacher()
test_gem_stanley_sympy_teacher()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment