theta_dot_max = 4
n_steps = 2
target = ztp.Box(np.array([[-.2, .2], [-.2, .2]]))
input_box_multistep = ztp.Box(
np.tile(np.array([[input_min, input_max]]), (n_steps, 1)))
state_box = ztp.Box(
np.array([[theta_min, theta_max], [theta_dot_min, theta_dot_max]]))
affine_dynamics = pwa.get_affine_dynamics(F, state_box, input_box)
reachset_func = lambda x, u: affine_dynamics.compute_reachable_set(x, u)
winning_check = WinSetCheck(state_box.get_range(), np.array([[0.01], [0.01]]))
winning_check.winset.set_patch(target.get_range(), 1)
# fig, ax = plt.subplots()
# im = ax.imshow(winning_check.winset.map)
# plt.show()
cells = pwa.StateCell(state_box.get_range())
input_list = list(np.arange(input_min, input_max, 0.02))
check_include = winning_check.is_included
check_intersect = winning_check.intersects
iterations = 5
# cells = H.state_cell.get_bare_copy()
cells = pwa.StateCell(state_box.get_range())
target_list = [target]
colors = ['g', 'y', 'k', 'r', 'b']
tot_time = time.time()
ztp.plot_zonotope(target)
for i in range(iterations):
start_time = time.time()
cells = pwa.compute_pre_rocs(reachset_func, cells, input_list,
# print("-------------")
min_cell_size = np.maximum(min_cell_size, F_linear_multistep.W.get_bounding_box_size())
# assert np.all(target.get_bounding_box_size() > min_cell_size)
# Control Synthesis
winning_set_resolved = []
winning_set_unresolved = []
partial_winning_set = []
cell_list = []
for i in np.arange(theta_min, theta_max, target.get_bounding_box_size()[0]):
for j in np.arange(theta_dot_min, theta_dot_max, target.get_bounding_box_size()[1]):
lb = np.array([[i], [j]])
ub = lb + target.get_bounding_box_size()
range = np.concatenate((lb, ub), axis=1)
new_cell = pwa.StateCell(range)
cell_list.append(deepcopy(new_cell))
ztp.plot_zonotope(new_cell.as_box(), color='k', fill=False)
winning_set_unresolved.append(pwa.StateCell(target.get_range()))
while winning_set_unresolved:
# if len(winning_set_unresolved) > 3:
# break
temp_target = winning_set_unresolved.pop()
winning_set_resolved.append(temp_target)
temp_box = temp_target.as_box()
temp_theta_range = temp_target.range[0, :]
theta_in = 0
best_region = -1
for r_id, r in enumerate(np.arange(theta_min, theta_max, region_theta_step)):