def binary_search_overshoot(
ys, save_filename="models/pstl_overshoot_binary_search.npy"):
ϵ_list = []
N = ys.shape[0]
for i in range(N):
y = torch.as_tensor(ys[i:i + 1, :]).float().unsqueeze(-1)
s = stlcg.Expression('s', y)
ϵL = torch.as_tensor(np.zeros([1, 1, 1])).float().requires_grad_(True)
ϵU = torch.as_tensor(2 *
np.ones([1, 1, 1])).float().requires_grad_(True)
ϵ = torch.as_tensor(np.ones([1, 1, 1])).float().requires_grad_(True)
ϕL = stlcg.Always(subformula=(s < ϵL))
ϕU = stlcg.Always(subformula=(s < ϵU))
ϕ = stlcg.Always(subformula=(s < ϵ))
while torch.abs(ϕU.subformula.val - ϕL.subformula.val) > 5 * 1E-3:
ϵ = 0.5 * (ϕU.subformula.val + ϕL.subformula.val)
ϕ.subformula.val = ϵ
r = ϕ.robustness(s).squeeze()
if r > 0:
ϕU.subformula.val = ϵ
else:
ϕL.subformula.val = ϵ
ϵ_list.append(ϵ.squeeze().detach().numpy())
if save_filename is None:
return np.stack(ϵ_list)
np.save(save_filename, np.stack(ϵ_list))
def binary_search_settling_vectorize(ys):
N = ys.shape[0]
y = torch.as_tensor(ys).float().unsqueeze(-1)
s = stlcg.Expression('s', torch.abs(y - 1))
ϵL = torch.as_tensor(np.zeros([N, 1, 1])).float().requires_grad_(True)
ϵU = torch.as_tensor(np.ones([N, 1, 1])).float().requires_grad_(True)
ϵ = torch.as_tensor(np.ones([N, 1, 1])).float().requires_grad_(True)
ϕL = stlcg.Always(subformula=(s < ϵL), interval=[50, 100])
ϕU = stlcg.Always(subformula=(s < ϵU), interval=[50, 100])
ϕ = stlcg.Always(subformula=(s < ϵ), interval=[50, 100])
while torch.abs(ϕU.subformula.val - ϕL.subformula.val).max() > 5 * 1E-3:
ϵ = 0.5 * (ϕU.subformula.val + ϕL.subformula.val)
ϕ.subformula.val = ϵ
r = ϕ.robustness(s)
ϕU.subformula.val = torch.where(
torch.abs(ϕU.subformula.val - ϕL.subformula.val) > 5 * 1E-3,
torch.where(r > 0, ϵ, ϕU.subformula.val), ϕU.subformula.val)
ϕL.subformula.val = torch.where(
torch.abs(ϕU.subformula.val - ϕL.subformula.val) > 5 * 1E-3,
torch.where(r <= 0, ϵ, ϕL.subformula.val), ϕL.subformula.val)
return ϵ.squeeze().detach().numpy()
def stlcg_overshoot(ys, save_filename="models/pstl_overshoot_stlcg.npy"):
N = ys.shape[0]
max_epoch = 1000
y = torch.as_tensor(ys).float().unsqueeze(-1)
s = stlcg.Expression('s', y)
ϵ = torch.as_tensor(np.zeros([N, 1, 1])).float().requires_grad_(True)
ϕ = stlcg.Always(subformula=(s < ϵ))
for epoch in range(max_epoch):
loss = torch.relu(-ϕ.robustness(s).squeeze()).sum()
loss.backward()
with torch.no_grad():
ϵ -= 0.005 * ϵ.grad
if loss == 0:
break
ϵ.grad.zero_()
if save_filename is None:
return ϵ.squeeze().cpu().detach().numpy()
np.save(save_filename, ϵ.squeeze().cpu().detach().numpy())
def stlcg_settling(ys, save_filename="models/pstl_settling_stlcg.npy"):
max_epoch = 1000
N = ys.shape[0]
y = torch.as_tensor(ys).float().unsqueeze(-1)
s = stlcg.Expression('s', torch.abs(y - 1))
ϵ = torch.as_tensor(np.zeros([N, 1, 1])).float().requires_grad_(True)
ϕ = stlcg.Always(subformula=(s < ϵ), interval=[50, 100])
for epoch in range(max_epoch):
loss = torch.relu(-ϕ.robustness(s).squeeze()).sum()
loss.backward()
with torch.no_grad():
ϵ -= 0.005 * ϵ.grad
ϵ.grad.zero_()
if loss == 0:
break
if save_filename is None:
return ϵ.squeeze().detach().numpy()
np.save(save_filename, ϵ.squeeze().detach().numpy())
p = sample_environment_parameters(case, 1)[0]
env = generate_env_from_parameters(case,
p,
carlength=params["lr"] + params["lf"])
# initial conditions and stl formula
if case == "coverage":
lower = torch.tensor(
[env.initial.lower[0], env.initial.lower[1], np.pi / 4, 0])
upper = torch.tensor(
[env.initial.upper[0], env.initial.upper[1], 3 * np.pi / 4, 2])
in_end_goal = inside_circle(env.final, "distance to final")
stop_in_end_goal = in_end_goal & (stlcg.Expression('speed') < 0.5)
end_goal = stlcg.Eventually(subformula=stlcg.Always(
subformula=stop_in_end_goal))
in_coverage = inside_circle(env.covers[0], "distance to coverage") & (
stlcg.Expression('speed') < 2.0)
coverage = stlcg.Eventually(
subformula=stlcg.Always(in_coverage, interval=[0, 8]))
avoid_obs = always_outside_circle(env.obs[0], "distance to obstacle")
formula = stlcg.Until(subformula1=coverage,
subformula2=end_goal) & avoid_obs
formula_input_func = lambda s, env: get_formula_input_coverage(
s, env, device, backwards=False)
xlim = [-6, 16]
ylim = [-6, 16]
elif case == "drive":
lower = torch.tensor(
[env.initial.lower[0], env.initial.lower[1], np.pi / 2, 0])
def always_outside_circle(circle, name, interval=None):
return stlcg.Always(subformula=outside_circle(circle, name),
interval=interval)
def always_inside_circle(cover, name, interval=[0, 5]):
return stlcg.Always(subformula=inside_circle(cover, name),
interval=interval)
