From ba4170fd49219ea22f33a11069bcd0fbf2d5c89e Mon Sep 17 00:00:00 2001
From: "Hsieh, Chiao" <chsieh16@illinois.edu>
Date: Mon, 18 Apr 2022 14:31:49 -0500
Subject: [PATCH] Add sympy teacher
---
gem_stanley_teacher.py | 129 +++++++++++++++++++++++++++++++++++------
1 file changed, 110 insertions(+), 19 deletions(-)
diff --git a/gem_stanley_teacher.py b/gem_stanley_teacher.py
index 6b43b2c..59c8ef0 100644
--- a/gem_stanley_teacher.py
+++ b/gem_stanley_teacher.py
@@ -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()
--
GitLab