def test_with_dataset():
#FIXME: Import issues
from utils.PWA_Control_utils import polytree_to_zonotope_tree
import pickle
with open("zonotope_datasets/inverted_pendulum.pkl", "rb") as f:
state_tree = pickle.load(f)
f.close()
# extract zonotopes
zonotope_tree = polytree_to_zonotope_tree(state_tree)
zonotope_count = len(zonotope_tree.zonotopes)
query_point = np.asarray([(np.random.rand(1) - 0.5) * 0.24,
(np.random.rand(1) - 0.5) * 2])
print(query_point)
query_point = query_point.reshape(-1, 1)
closest_zonotope, candidate_boxes, query_box = zonotope_tree.find_closest_polytopes(query_point)
print(('Query point: ', query_point))
ax_lim = np.asarray([-zonotope_count, zonotope_count, -zonotope_count, zonotope_count]) * 1.1
fig, ax = visZ(zonotope_tree.zonotopes, title="", alpha=0.2, axis_limit=ax_lim)
fig, ax = visZ(closest_zonotope, title="", fig=fig, ax=ax, alpha=1, axis_limit=ax_lim)
fig, ax = visualize_box_nodes(zonotope_tree.box_nodes, fig=fig, ax=ax, alpha=0.4, linewidth=0.5)
fig, ax = visualize_boxes(candidate_boxes, fig=fig, ax=ax, alpha=1)
print(('Evaluating %d zonotopes out of %d' % (len(candidate_boxes), len(zonotope_tree.zonotopes))))
fig, ax = visualize_boxes([query_box], fig=fig, ax=ax, alpha=0.3, fill=True)
plt.scatter(query_point[0], query_point[1], s=20, color='k')
print(('Closest Zonotope: ', closest_zonotope))
plt.show()
def visualize(self):
"""
Assumption: all AH-polytopes are zonotopes, and the problem is in 2D
"""
fig, ax = plt.subplots(
) # note we must use plt.subplots, not plt.subplot
zonotopes = []
for leaf in self.leafs:
color = (random.random(), random.random(), random.random())
zonotopes.extend([
zonotope(poly.x, poly.G, color=color)
for poly in leaf.list_of_polytopes
])
for hyperplane in self.hyperplanes:
x_min, x_max = -200, 200
c, g = hyperplane[0:2]
g = np.asscalar(g)
# print c,c.shape,g
ax.plot([x_min, x_max],
[(g - x_min * c[0, 0]) / (c[1, 0] + 0.001),
(g - x_max * c[0, 0]) / (c[1, 0] + 0.001)],
color=(0.2, 0, 0),
linewidth=1,
linestyle='dashed')
visZ(ax, zonotopes, title="BSP_tree")
def draw_leaf(self, leaf):
"""
Assumption: all AH-polytopes are zonotopes, and the problem is in 2D
"""
# assert leaf in self.cells
fig, ax = plt.subplots(
) # note we must use plt.subplots, not plt.subplot
for P in self.list_of_polytopes:
if P in leaf.list_of_polytopes:
P.color = (0, 0, 1)
else:
P.color = (1, 0, 0)
vis(ax, [leaf.polytope])
visZ(ax,
self.list_of_polytopes,
title="BSP_tree for %s" % leaf.__repr__())
def zonotope_reduction_box_nd(zonotope_count, dim, num_of_queries):
zonotopes = []
centroid_range = zonotope_count*8
generator_range = zonotope_count
print(('Centroid range: %d' %centroid_range))
print(('Generator range: %d' %generator_range))
for i in range(zonotope_count):
m = np.random.random_integers(dim, 2*dim)
G = (np.random.rand(dim, m) - 0.5) * generator_range * 1
x = 2*(np.random.rand(dim,1) - 0.5)*centroid_range
# print(x)
zonotopes.append(zonotope(x, G))
zt = PolytopeTree_Old(zonotopes)
#query points in a line
# query_points = (np.random.rand(dim-1,num_of_queries)-0.5)
# query_points = np.vstack([query_points, 2*(np.random.rand(1, num_of_queries) - 0.5) * centroid_range*2])
#query points anywhere in space
query_points = 2 * (np.random.rand(dim, num_of_queries) - 0.5) * centroid_range * 2
reduction_ratios = np.zeros([num_of_queries])
for i, q in enumerate(query_points.T):
closest_zonotope, candidate_boxes, query_box = zt.find_closest_polytopes(q)
reduction_ratios[i] = len(candidate_boxes)/(zonotope_count*1.)
if dim ==2:
fig, ax = visZ(zt.polytopes, title="", alpha=0.2)
ax.scatter(query_points[0,:], query_points[1,:],s=3)
plt.xlabel('x')
plt.ylabel('y')
plt.title('Zonotope and Query Point Distribution')
plt.show()
return np.average(reduction_ratios), np.std(reduction_ratios)
def zonotope_reduction_line_nd(zonotope_count, dim, num_of_queries):
zonotopes = []
centroid_range = zonotope_count*5
generator_range = 10
for i in range(zonotope_count):
m = np.random.random_integers(dim, 2*dim)
G = 2*(np.random.rand(dim, m) - 0.5) * generator_range * 1
x = 2*(np.random.rand(dim-1,1) - 0.5)
x = np.vstack([2*(np.random.rand(1,1) - 0.5) * centroid_range,x])
# print(x)
zonotopes.append(zonotope(x, G))
zt = PolytopeTree_Old(zonotopes)
query_points = (np.random.rand(dim-1,num_of_queries)-0.5)
query_points = np.vstack([2*(np.random.rand(1, num_of_queries) - 0.5) * centroid_range,query_points])
# print(query_points)
reduction_ratios = np.zeros([num_of_queries])
for i, q in enumerate(query_points.T):
closest_zonotope, candidate_boxes, query_box = zt.find_closest_polytopes(q)
reduction_ratios[i] = len(candidate_boxes)/(zonotope_count*1.)
if dim ==2:
fig, ax = visZ(zt.polytopes, title="", alpha=0.2)
ax.scatter(query_points[0,:], query_points[1,:],s=3)
ax.set_xlim([-centroid_range*1.2,centroid_range*1.2])
ax.set_ylim([-centroid_range * 1.2, centroid_range * 1.2])
plt.xlabel('x')
plt.ylabel('y')
plt.title('Zonotope and Query Point Distribution')
plt.show()
return np.average(reduction_ratios), np.std(reduction_ratios)
def visualize_tree(tree):
fig, ax = plt.subplots()
axis_limit = [-0.15, 0.15, -1.2, 1.2]
visZ(ax, [x.p for x in tree.states],
axis_limit=axis_limit,
title="%s - %d zonotopes %d branches" %
(tree.name, len(tree.states), len(tree.branches)))
ax.set_xlabel(r"$\theta$", fontdict=font)
h = ax.set_ylabel(r"$\dot{\theta}$", fontdict=font)
h.set_rotation(0)
ax.plot([0.1, 0.1], [-1, 1],
color=(0, 0, 0),
linewidth=2,
linestyle='dashed')
ax.plot([0.12, 0.12, -0.12, -0.12, 0.12], [-1, 1, 1, -1, -1],
color=(0, 0, 0),
linewidth=2)
def test_many_zonotopes_line(self):
zonotope_count = 50
zonotopes = []
centroid_range = zonotope_count
generator_range = 3
for i in range(zonotope_count):
m = np.random.random_integers(4, 10)
G = (np.random.rand(2, m) - 0.5) * generator_range * 1
x = np.asarray([(np.random.rand(1) - 0.5) * 2 * centroid_range,
np.random.rand(1) - 0.5])
zonotopes.append(zonotope(x, G))
zt = PolytopeTree_Old(zonotopes)
query_point = np.asarray([
np.random.random_integers(-centroid_range, centroid_range),
np.random.rand(1) - 0.5
])
query_point = query_point.reshape(-1, 1)
closest_zonotope, candidate_boxes, query_box = zt.find_closest_zonotopes(
query_point)
print(('Query point: ', query_point))
ax_lim = np.asarray([
-centroid_range, centroid_range, -centroid_range, centroid_range
]) * 1.1
fig, ax = visZ(zonotopes, title="", alpha=0.2, axis_limit=ax_lim)
fig, ax = visZ(closest_zonotope,
title="",
fig=fig,
ax=ax,
alpha=1,
axis_limit=ax_lim)
fig, ax = visualize_box_nodes(zt.box_nodes,
fig=fig,
ax=ax,
alpha=0.4,
linewidth=0.5)
fig, ax = visualize_boxes(candidate_boxes, fig=fig, ax=ax, alpha=1)
print(('Candidate boxes: ', candidate_boxes))
fig, ax = visualize_boxes([query_box],
fig=fig,
ax=ax,
alpha=0.3,
fill=True)
plt.scatter(query_point[0], query_point[1], s=20, color='k')
print(('Closest Zonotope: ', closest_zonotope))
plt.show()
def test_few_zonotopes(self):
G_l = np.array([[1, 0, 0, 3], [0, 1, 2, -1]]) * 0.8
G_r = np.array([[1, 0, 1, 1, 2, -2], [0, 1, 1, -1, 5, 2]]) * 1
x_l = np.array([0., 1.]).reshape(2, 1)
x_r = np.array([5., 0.]).reshape(2, 1)
zono_l = zonotope(x_l, G_l)
zono_r = zonotope(x_r, G_r)
zt = PolytopeTree([zono_l, zono_r])
query_point = np.asarray([0, -5])
np.reshape(query_point, (query_point.shape[0], 1))
closest_zonotope, best_distance, evaluated_zonotopes, query_point = zt.find_closest_polytopes(
np.asarray(query_point), return_intermediate_info=True)
# print(closest_zonotope)
fig, ax = visZ([zono_r, zono_l], title="", alpha=0.2)
plt.scatter(query_point[0], query_point[1])
fig, ax = visZ(closest_zonotope, title="", fig=fig, ax=ax, alpha=0.75)
# fig, ax = visualize_box_nodes(zt,fig=fig,ax=ax,alpha =0.4)
print(('Closest Zonotope: ', closest_zonotope))
plt.show()
def test_zonotope_to_AABB(self):
G_l = np.array([[1, 0, 0, 3], [0, 1, 2, -1]]) * 0.8
G_r = np.array([[1, 0, 1, 1, 2, -2], [0, 1, 1, -1, 5, 2]]) * 1
x_l = np.array([0, 1]).reshape(2, 1)
x_r = np.array([1, 0]).reshape(2, 1)
zono_l = zonotope(x_l, G_l)
zono_r = zonotope(x_r, G_r)
AABB_r = zonotope_to_box(zono_r)
AABB_l = zonotope_to_box(zono_l)
fix, ax = visZ([zono_r,zono_l], title="")
visualize_boxes([AABB_r,AABB_l], ax= ax)
plt.show()
A[3] = np.array([[1, dt], [-0.5, 1.2]])
B[3] = np.array([[0, dt]]).T
W[3] = np.array([[1, 0], [0, 1]]) * dt * 0.2
s = system_T_periodic(A, B, W)
s.X = zonotope(np.zeros((2, 1)), np.eye(2) * 2)
s.U = zonotope(np.zeros((1, 1)), np.eye(1) * 3)
G, theta = RCI_periodic(s, q=12)
T = s.T + 1
for t in range(T + 1):
if t == T:
visZ([
zonotope(np.zeros((2, 1)),
G[t],
color=(t / (s.T + 1.5), 0, 1 - t / (s.T + 1.5)))
],
a=0.02,
title=r"$\mathcal{Z}(0,G_{0})=\mathcal{Z}(0,G_{%d})$" % T)
else:
visZ([
zonotope(np.zeros((2, 1)),
G[t],
color=(t / (s.T + 1.5), 0, 1 - t / (s.T + 1.5)))
],
a=0.02,
title=r"$\mathcal{Z}(0,G_{%d})$" % t)
def test_many_zonotopes(self,
distance_scaling_array=np.ones(2, dtype='float')):
zonotope_count = 15
centroid_range = zonotope_count * 1.5
# seed = int(time())
seed = np.random.random_integers(0, 10000000, 1) # int(time())
np.random.seed(seed)
zonotopes = get_uniform_random_zonotopes(
zonotope_count,
dim=2,
generator_range=zonotope_count * 0.3,
centroid_range=centroid_range,
return_type='zonotope',
seed=seed)
zt = PolytopeTree(zonotopes,
distance_scaling_array=distance_scaling_array)
query_point = np.asarray([
np.random.random_integers(-centroid_range, centroid_range),
np.random.random_integers(-centroid_range, centroid_range)
])
query_point = query_point.reshape(-1, 1)
closest_zonotope, best_distance, evaluated_zonotopes, query_box = zt.find_closest_polytopes(
query_point, return_intermediate_info=True)
print(('Solved %d LP' % len(evaluated_zonotopes)))
print(('Best distance: ', best_distance))
print(('Query point: ', query_point))
ax_lim = np.asarray([
-centroid_range, centroid_range, -centroid_range, centroid_range
]) * 1.1
fig, ax = visZ(zonotopes,
title="",
alpha=0.2,
axis_limit=ax_lim,
color='black')
fig, ax = visualize_boxes(
[zonotope_to_box(p, return_AABB=False) for p in zt.polytopes],
fig=fig,
ax=ax,
alpha=0.08,
linewidth=0.5,
facecolor='black')
fig, ax = visualize_boxes([
zonotope_to_box(p, return_AABB=False) for p in evaluated_zonotopes
],
fig=fig,
ax=ax,
alpha=0.2,
linewidth=0.5,
facecolor='red')
fig, ax = visZ(closest_zonotope,
title="",
fig=fig,
ax=ax,
alpha=1,
axis_limit=ax_lim,
color='blue')
for vertex in zt.scaled_key_point_tree.data:
plt.scatter(*np.divide(vertex, distance_scaling_array),
facecolor='c',
s=2,
alpha=1)
# fig, ax = visualize_boxes(candidate_boxes,fig=fig,ax=ax,alpha =1)
# print('Candidate boxes: ', candidate_boxes)
fig, ax = visualize_boxes([query_box],
fig=fig,
ax=ax,
alpha=0.3,
xlim=[-centroid_range, centroid_range],
ylim=[-centroid_range, centroid_range],
facecolor='cyan')
# lim = 60
# ax.set_xlim(-lim,lim)
# ax.set_ylim(-lim,lim)
ax.xaxis.set_major_locator(MultipleLocator(15))
ax.yaxis.set_major_locator(MultipleLocator(15))
ax.set_title('Nearest Polytope Querying with AABB')
plt.gca().set_aspect('equal', adjustable='box')
plt.tight_layout()
ax.scatter(query_point[0], query_point[1], s=10, color='k')
print(('Closest Zonotope: ', closest_zonotope))
plt.savefig('closest_zonotope.png', dpi=300)
plt.close()
def test_insertion(self):
zonotope_count = 10
insert_count = 5
centroid_range = zonotope_count * 6
seed = np.random.random_integers(0, 10000000, 1) # int(time())
print(('Seed: ', seed))
all_zonotopes = get_uniform_random_zonotopes(
zonotope_count + insert_count,
dim=2,
generator_range=zonotope_count * 1,
centroid_range=centroid_range,
return_type='zonotope',
seed=seed)
insert_zonotopes = all_zonotopes[zonotope_count:]
zonotopes = all_zonotopes[0:zonotope_count]
zt = PolytopeTree(zonotopes)
query_point = np.asarray([
np.random.random_integers(-centroid_range, centroid_range),
np.random.random_integers(-centroid_range, centroid_range)
])
query_point = query_point.reshape(-1, 1)
closest_zonotope, best_distance, evaluated_zonotopes, query_box = zt.find_closest_polytopes(
query_point, return_intermediate_info=True)
print(('Solved %d LP' % len(evaluated_zonotopes)))
print(('Best distance: ', best_distance))
print(('Query point: ', query_point))
ax_lim = np.asarray([
-centroid_range, centroid_range, -centroid_range, centroid_range
]) * 1.1
fig, ax = visZ(zonotopes, title="", alpha=0.2, axis_limit=ax_lim)
fig, ax = visZ(closest_zonotope,
title="",
fig=fig,
ax=ax,
alpha=1,
axis_limit=ax_lim)
# fig, ax = visualize_box_nodes(zt.box_nodes,fig=fig,ax=ax,alpha =0.4,linewidth=0.5)
for vertex in zt.scaled_key_point_tree.data:
plt.scatter(vertex[0], vertex[1], facecolor='c', s=2, alpha=1)
# fig, ax = visualize_boxes(candidate_boxes,fig=fig,ax=ax,alpha =1)
# print('Candidate boxes: ', candidate_boxes)
fig, ax = visualize_boxes([query_box],
fig=fig,
ax=ax,
alpha=0.1,
xlim=[-centroid_range, centroid_range],
ylim=[-centroid_range, centroid_range])
# ax.set_xlim(-centroid_range,centroid_range)
# ax.set_ylim(-centroid_range,centroid_range)
ax.scatter(query_point[0], query_point[1], s=10, color='k')
print(('Closest Zonotope: ', closest_zonotope))
plt.show()
#insert
zt.insert(insert_zonotopes)
closest_zonotope, best_distance, evaluated_zonotopes, query_box = zt.find_closest_polytopes(
query_point, return_intermediate_info=True)
print(('Solved %d LP' % len(evaluated_zonotopes)))
print(('Best distance: ', best_distance))
print(('Query point: ', query_point))
ax_lim = np.asarray([
-centroid_range, centroid_range, -centroid_range, centroid_range
]) * 1.1
fig, ax = visZ(all_zonotopes, title="", alpha=0.2, axis_limit=ax_lim)
fig, ax = visZ(closest_zonotope,
title="",
fig=fig,
ax=ax,
alpha=1,
axis_limit=ax_lim)
# fig, ax = visualize_box_nodes(zt.box_nodes,fig=fig,ax=ax,alpha =0.4,linewidth=0.5)
for vertex in zt.scaled_key_point_tree.data:
plt.scatter(vertex[0], vertex[1], facecolor='c', s=2, alpha=1)
print((len(zt.scaled_key_point_tree.data)))
# fig, ax = visualize_boxes(candidate_boxes,fig=fig,ax=ax,alpha =1)
# print('Candidate boxes: ', candidate_boxes)
fig, ax = visualize_boxes([query_box],
fig=fig,
ax=ax,
alpha=0.1,
xlim=[-centroid_range, centroid_range],
ylim=[-centroid_range, centroid_range])
# ax.set_xlim(-centroid_range,centroid_range)
# ax.set_ylim(-centroid_range,centroid_range)
ax.scatter(query_point[0], query_point[1], s=10, color='k')
print(('Closest Zonotope: ', closest_zonotope))
plt.show()
B = Box(q)
N = 200
X_1 = translate(Box(n, 12), np.ones((n, 1)) * 10)
H = np.array([[1, 0], [0, 1], [-1, 0], [0, -1], [1, 1]]).reshape(5, 2)
h = np.array([42, 5, -2, -2, 35]).reshape(5, 1)
X_1 = polytope(H, h)
#X_2=translate(Box(n,5),np.ones((n,1))*15)
#X_3=translate(Box(n,4),np.array([[15,5]]).reshape(2,1))
#X_3=translate(Box(n,4),np.array([[15,5]]).reshape(2,1))
s = 2
#list_of_zonotopes=[zonotope(np.random.random((n,1))*10+np.array([0.1*i,0.5*i]).reshape(2,1),np.random.random((n,q))*1) for i in range(N)]
list_of_zonotopes = [
zonotope(
np.random.random((n, 1)) * np.array([40, 3]).reshape(2, 1),
np.random.random((n, q)) * s - s / 2) for i in range(N)
]
for Z in list_of_zonotopes:
Z.J = np.random.random()
fig, ax = plt.subplots() # note we must use plt.subplots, not plt.subplot
visZ(ax, list_of_zonotopes)
mytree = BSP_tree_cells([X_1], list_of_zonotopes)
mytree.construct_tree(D=10, N=5)
mytree.draw_cells(alpha=0.8)
x = np.array([8, 3]).reshape(2, 1)
(Z, D) = mytree.query_closest(x)
C = mytree._query_find_cell(x)
mytree.draw_leaf(C)
def visualize(self, axis_limit=[True]):
visZ(self.ax, [x.p for x in self.states],
axis_limit=axis_limit,
title="%s - %d zonotopes %d branches" %
(self.name, len(self.states), len(self.branches)))
self.fig.savefig("figures/tree_%d" % len(self.branches))
def test_voronoi_closest_zonotope(zonotope_count=30,
seed=None,
save=True,
key_vertices_count=3):
AH_polytopes = get_uniform_random_zonotopes(zonotope_count, dim=2, generator_range=zonotope_count*1.5,return_type='zonotope',\
centroid_range=zonotope_count*4, seed=seed)
zonotopes = get_uniform_random_zonotopes(zonotope_count, dim=2, generator_range=zonotope_count*1.5,return_type='zonotope',\
centroid_range=zonotope_count*4, seed=seed)
#precompute
vca = VoronoiClosestPolytope(AH_polytopes, key_vertices_count)
#build query point
query_point = (np.random.rand(2) - 0.5) * zonotope_count * 5
print(('Query point ' + str(query_point)))
np.reshape(query_point, (query_point.shape[0], 1))
#query
closest_polytope, best_distance, closest_AHpolytope_candidates = vca.find_closest_polytope(
query_point, return_intermediate_info=True)
#find indices for plotting
candidate_indices = np.zeros(len(closest_AHpolytope_candidates),
dtype='int')
closest_index = np.zeros(1, dtype='int')
for i, cac in enumerate(closest_AHpolytope_candidates):
for j in range(len(AH_polytopes)):
if cac == AH_polytopes[j]:
candidate_indices[i] = j
break
for j in range(len(AH_polytopes)):
if closest_polytope == AH_polytopes[j]:
closest_index[0] = j
break
# print(candidate_indices)
#visualize voronoi
# fig = voronoi_plot_2d(vca.centroid_voronoi, point_size=2,show_vertices=False, line_alpha=0.4, line_width=1)
fig = plt.figure()
ax = fig.add_subplot(111)
# dist, voronoi_centroid_index = vca.centroid_tree.query(query_point)
print(('Checked %d polytopes' % len(closest_AHpolytope_candidates)))
#visualize vertex circles
# #sanity check
# vertex = vca.centroid_voronoi.vertices[15]
# for centroid_index in vca.vertex_to_voronoi_centroid_index[str(vertex)]:
# centroid = vca.centroid_voronoi.points[centroid_index]
# plt.scatter(centroid[0], centroid[1], facecolor='green', s=15)
# plt.scatter(vertex[0], vertex[1], facecolor='green', s=15)
# voronoi_centroid = vca.centroid_voronoi.points[voronoi_centroid_index]
# print('Centroid dist '+str(dist))
# print('Voronoi Centroid index ' + str(voronoi_centroid_index))
# print('Closest Voronoi centroid ' + str(vca.centroid_tree.data[voronoi_centroid_index,:]))
# print('Closest polytope centroid ' + str(evaluated_zonotopes[0].x))
# plt.scatter(voronoi_centroid[0], voronoi_centroid[1], facecolor='blue', s=6)
#visualize centroids
for vertex in vca.key_points:
plt.scatter(vertex[0], vertex[1], facecolor='c', s=2, alpha=1)
#visualize polytopes
fig, ax = visZ(zonotopes[candidate_indices],
title="",
fig=fig,
ax=ax,
alpha=0.3,
color='pink')
fig, ax = visZ(zonotopes[closest_index],
title="",
fig=fig,
ax=ax,
alpha=0.8,
color='brown')
fig, ax = visZ(zonotopes,
title="",
alpha=0.07,
fig=fig,
ax=ax,
color='gray')
plt.scatter(query_point[0], query_point[1], facecolor='red', s=6)
plt.axes().set_aspect('equal')
plt.xlabel('$x$')
plt.ylabel('$y$')
plt.title('Closest Zonotope with Voronoi Diagram')
print(('Closest Zonotope: ', closest_polytope))
if save:
plt.savefig('closest_zonotope' + str(default_timer()) + '.png',
dpi=500)
else:
plt.show()
from pypolycontain.lib.zonotope import zonotope
dt=0.05
A=np.array([[1,dt],[0,1]])
B=np.array([[0,dt]]).T
W=np.array([[1,0],[0,1]])*dt*0.2
s=system_LTI(A,B,W)
s.X=zonotope(np.zeros((2,1)), np.eye(2) *2 )
s.U=zonotope(np.zeros((1,1)), np.eye(1) *3 )
alpha=0.5
phi,theta=RCI(s,q=10,alpha=alpha)
visZ([zonotope(np.zeros((2,1)),s.phi,color=(1,0,0))],a=0.02,title=r"RCI set")
#visZ([zonotope(np.zeros((1,1)),theta*1/(1-alpha))])
# Simulate
T=200
x,u={},{}
x[0]=np.array([-0.08,0.3]).reshape(2,1)
for t in range(T):
print t,x[t].T
u[t]=RCI_controller(s,x[t])
zeta_w=np.random.random((2,1))*2-1
x[t+1]=np.dot(A,x[t])+np.dot(B,u[t])+np.dot(W,zeta_w)
"""
Visualization
"""
from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes_ax as visZ
from PWA_lib.polytree.bsp.bsp import create_hyperplane, BSP_tree
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
n = 2
q = 3
B = Box(q)
N = 100
X = translate(Box(2, N / 2.0 + 2), np.array([N / 2, N / 2]).reshape(2, 1))
list_of_polytopes = [
AH_polytope(
np.random.random((n, q)) * 1,
np.random.random((n, 1)) * 10 + 1 * i, B) for i in range(N)
]
zonotopes = {poly: zonotope(poly.t, poly.T) for poly in list_of_polytopes}
fig, ax = plt.subplots() # note we must use plt.subplots, not plt.subplot
visZ(ax, zonotopes.values())
mytree = BSP_tree(list_of_polytopes)
mytree.construct_tree(6, N=3)
mytree._get_polyhedrons(X)
P_list = mytree.build_Plist()
for leaf in mytree.leafs:
print leaf, len(P_list[leaf])
