def find_important_dimensions(self, poly1, poly2):
'''assuming check_contained(poly1,poly2) returns true, we are interested in the halfspaces that matters
poly1 = root, poly2 = candidate
'''
# #Binary Space Partitioning
gurobi_model = grb.Model()
gurobi_model.setParam('OutputFlag', self.output_flag)
input1 = Experiment.generate_input_region(gurobi_model, self.analysis_template, poly1, self.env_input_size)
relevant_directions = []
for j, template in enumerate(self.analysis_template):
multiplication = 0
for i in range(self.env_input_size):
multiplication += template[i] * input1[i]
previous_constraint = gurobi_model.getConstrByName("check_contained_constraint")
if previous_constraint is not None:
gurobi_model.remove(previous_constraint)
gurobi_model.update()
gurobi_model.addConstr(multiplication <= poly2[j], name=f"check_contained_constraint")
gurobi_model.update()
x_results = self.optimise(self.analysis_template, gurobi_model, input1)
if np.allclose(np.array(poly1), x_results) is False:
vertices = np.stack(self.pypoman_compute_polytope_vertices(self.analysis_template, np.array(x_results)))
samples = polytope.sample(1000, self.analysis_template, x_results)
from scipy.spatial import ConvexHull
hull = ConvexHull(samples)
volume = hull.volume # estimated volume
relevant_directions.append((j, volume))
return relevant_directions
def generate_root_polytope(self, input_boundaries):
gurobi_model = grb.Model()
gurobi_model.setParam('OutputFlag', self.output_flag)
input = Experiment.generate_input_region(gurobi_model, self.input_template, input_boundaries, self.env_input_size)
x_results = self.optimise(self.analysis_template, gurobi_model, input)
if x_results is None:
print("Model unsatisfiable")
return None
root = tuple(x_results)
return root
def check_contained(self, poly1, poly2):
gurobi_model = grb.Model()
gurobi_model.setParam('OutputFlag', self.output_flag)
input1 = Experiment.generate_input_region(gurobi_model, self.analysis_template, poly1, self.env_input_size)
Experiment.generate_region_constraints(gurobi_model, self.analysis_template, input1, poly2, self.env_input_size, eps=1e-5) # use epsilon to prevent single points
x_results = self.optimise(self.analysis_template, gurobi_model, input1)
if x_results is None:
# not contained
return False
else:
return True
def create_range_bounds_model(self, template, x, env_input_size, nn, round=-1):
gurobi_model = grb.Model()
gurobi_model.setParam('OutputFlag', False)
input = Experiment.generate_input_region(gurobi_model, template, x, env_input_size)
gurobi_model.update()
gurobi_model.optimize()
assert gurobi_model.status == 2, "LP wasn't optimally solved"
observation = self.get_observation_variable(input, gurobi_model) # get the observation from the input
ranges = Experiment.get_range_bounds(observation, nn, gurobi_model)
if self.use_softmax:
ranges_probs = unroll_methods.softmax_interval(ranges)
else:
ranges_probs = ranges
if round >= 0:
pass
# todo round the probabilities
return ranges_probs
def post_milp(self, x, x_label, nn, output_flag, t, template):
"""milp method"""
post = []
for chosen_action in range(2):
gurobi_model = grb.Model()
gurobi_model.setParam('OutputFlag', output_flag)
input = Experiment.generate_input_region(gurobi_model, template, x,
self.env_input_size)
# gurobi_model.addConstr(input[0] >= 0, name=f"input_base_constr1")
# gurobi_model.addConstr(input[1] >= 0, name=f"input_base_constr2")
# gurobi_model.addConstr(input[2] >= 20, name=f"input_base_constr3")
observation = gurobi_model.addMVar(shape=(2, ),
lb=float("-inf"),
ub=float("inf"),
name="observation")
gurobi_model.addConstr(
observation[1] <= input[0] - input[1] + self.input_epsilon / 2,
name=f"obs_constr21") # delta_x
gurobi_model.addConstr(
observation[1] >= input[0] - input[1] - self.input_epsilon / 2,
name=f"obs_constr22")
gurobi_model.addConstr(observation[0] <= self.v_lead - input[2] +
self.input_epsilon / 2,
name=f"obs_constr11") # delta_v
gurobi_model.addConstr(observation[0] >= self.v_lead - input[2] -
self.input_epsilon / 2,
name=f"obs_constr12")
# gurobi_model.addConstr(input[3] <= self.max_speed, name=f"v_constr_input")
# gurobi_model.addConstr(input[3] >= -self.max_speed, name=f"v_constr_input")
feasible_action = Experiment.generate_nn_guard(
gurobi_model, observation, nn, action_ego=chosen_action, M=1e6)
# feasible_action = Experiment.generate_nn_guard(gurobi_model, input, nn, action_ego=chosen_action)
if feasible_action:
# apply dynamic
x_prime = StoppingCarExperiment.apply_dynamic(
input,
gurobi_model,
action=chosen_action,
env_input_size=self.env_input_size)
gurobi_model.update()
gurobi_model.optimize()
found_successor, x_prime_results = self.h_repr_to_plot(
gurobi_model, template, x_prime)
if found_successor:
post.append((tuple(x_prime_results), (x, x_label)))
return post
def main_loop(self, nn, template, template_2d):
assert self.post_fn_continuous is not None
root = self.generate_root_polytope()
root_list = [root]
vertices_list = defaultdict(list)
seen = []
frontier = [(0, x) for x in root_list]
max_t = 0
num_already_visited = 0
widgets = [progressbar.Variable('n_workers'), ', ', progressbar.Variable('frontier'), ', ', progressbar.Variable('seen'), ', ', progressbar.Variable('num_already_visited'), ", ",
progressbar.Variable('max_t'), ", ", progressbar.Variable('last_visited_state')]
last_time_plot = None
if self.before_start_fn is not None:
self.before_start_fn(nn)
self.internal_model = grb.Model()
self.internal_model.setParam('OutputFlag', self.output_flag)
input = Experiment.generate_input_region(self.internal_model, self.input_template, self.input_boundaries, self.env_input_size)
self.last_input = input
with progressbar.ProgressBar(widgets=widgets) if self.show_progressbar else nullcontext() as bar:
while len(frontier) != 0:
t, x = frontier.pop(0) if self.use_bfs else frontier.pop()
if max_t > self.time_horizon:
print(f"Reached horizon t={t}")
self.plot_fn(vertices_list, template, template_2d)
return max_t, num_already_visited, vertices_list, False
contained_flag = False
to_remove = []
for s in seen:
if contained(x, s):
contained_flag = True
break
if contained(s, x):
to_remove.append(s)
for rem in to_remove:
num_already_visited += 1
seen.remove(rem)
if contained_flag:
num_already_visited += 1
continue
max_t = max(max_t, t)
vertices_list[t].append(np.array(x))
if self.check_unsafe(template, x):
print(f"Unsafe state found at timestep t={t}")
print(x)
self.plot_fn(vertices_list, template, template_2d)
return max_t, num_already_visited, vertices_list, True
seen.append(x)
x_primes_list = self.post_fn_continuous(x, nn, self.output_flag, t, template)
if last_time_plot is None or time.time() - last_time_plot >= self.plotting_time_interval:
if last_time_plot is not None:
self.plot_fn(vertices_list, template, template_2d)
last_time_plot = time.time()
if self.update_progress_fn is not None:
self.update_progress_fn(seen=len(seen), frontier=len(frontier), num_already_visited=num_already_visited, max_t=max_t)
if self.show_progressbar:
bar.update(value=bar.value + 1, seen=len(seen), frontier=len(frontier), num_already_visited=num_already_visited, last_visited_state=str(x), max_t=max_t)
assert len(x_primes_list) != 0, "something is wrong with the calculation of the successor"
# for x_primes in x_primes_list:
for x_prime in x_primes_list:
if self.use_rounding:
# x_prime_rounded = tuple(np.trunc(np.array(x_prime) * self.rounding_value) / self.rounding_value) # todo should we round to prevent numerical errors?
x_prime_rounded = self.round_tuple(x_prime, self.rounding_value)
# x_prime_rounded should always be bigger than x_prime
assert contained(x_prime, x_prime_rounded)
x_prime = x_prime_rounded
frontier = [(u, y) for u, y in frontier if not contained(y, x_prime)]
if not any([contained(x_prime, y) for u, y in frontier]):
frontier.append(((t + 1), x_prime))
# print(x_prime)
else:
num_already_visited += 1
self.plot_fn(vertices_list, template, template_2d)
return max_t, num_already_visited, vertices_list, False