def test_chull():
'tests aggregation of a cirle of sets using convex hull'
mode = HybridAutomaton().new_mode('mode_name')
r = 1.0
eps = 0.05
num_sets = 16
lpi_list = []
for theta in np.linspace(0, 2*math.pi, num_sets, endpoint=False):
y = r * math.sin(theta)
x = r * math.cos(theta)
mat = [[1, 0], [-1, 0], [0, 1], [0, -1]]
rhs = [x + eps, -(x - eps), y + eps, -(y - eps)]
lpi = lputil.from_constraints(mat, rhs, mode)
lpi_list.append(lpi)
#verts = []
#for lpi in lpi_list:
# verts += lpplot.get_verts(lpi)
# xs, ys = zip(*lpplot.get_verts(lpi))
# plt.plot(xs, ys, 'k-')
lpi = lputil.aggregate_chull(lpi_list, mode)
#xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'r--')
#for vert in verts:
# assert lputil.is_point_in_lpi(vert, lpi)
#plt.show()
# test if it's really convex hull
for theta in np.linspace(0, 2*math.pi, num_sets):
y = (r + 2*eps) * math.sin(theta)
x = (r + 2*eps) * math.cos(theta)
assert not lputil.is_point_in_lpi([x, y], lpi)
y = (r - 2*eps) * math.sin(theta)
x = (r - 2*eps) * math.cos(theta)
assert lputil.is_point_in_lpi([x, y], lpi)
def test_chull_ha5():
'test convex hull aggregation of harmonic oscillator with 5 sets'
mode = HybridAutomaton().new_mode('mode_name')
steps = 5
step_size = math.pi/4
lpi_list = []
a_mat = np.array([[0, 1], [-1, 0]], dtype=float)
for step_num in range(steps):
box = [[-5, -4], [-0.5, 0.5]]
lpi = lputil.from_box(box, mode)
t = step_num * step_size
basis_mat = expm(a_mat * t)
lputil.set_basis_matrix(lpi, basis_mat)
lpi_list.append(lpi)
verts = []
for lpi in lpi_list:
verts += lpplot.get_verts(lpi)
xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'k-')
lpi = lputil.aggregate_chull(lpi_list, mode)
#xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'r--')
#plt.show()
# test if it's really convex hull
assert lputil.is_point_in_lpi([0, 4.5], lpi)
for vert in verts:
assert lputil.is_point_in_lpi(vert, lpi)
def test_rotated_aggregate():
'tests rotated aggregation'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[0, 1], [0, 1]], mode)
lpi2 = lputil.from_box([[1, 2], [1, 2]], mode)
sq2 = math.sqrt(2) / 2.0
agg_dirs = np.array([[sq2, sq2], [sq2, -sq2]], dtype=float)
lpi = lputil.aggregate([lpi1, lpi2], agg_dirs, mode)
assert lputil.is_point_in_lpi([0, 0], lpi)
assert lputil.is_point_in_lpi([2, 2], lpi)
assert lputil.is_point_in_lpi([1, 2], lpi)
assert lputil.is_point_in_lpi([2, 1], lpi)
assert lputil.is_point_in_lpi([0, 1], lpi)
assert lputil.is_point_in_lpi([1, 0], lpi)
verts = lpplot.get_verts(lpi)
assert len(verts) == 5
for p in [(0.5, -0.5), (-0.5, 0.5), (2.5, 1.5), (1.5, 2.5)]:
assert pair_almost_in(p, verts)
assert verts[0] == verts[-1]
def test_aggregate_on_subspace():
'''
test aggregation when the dynamics and sets are only on a subspace.
'''
# dynamics are x' == 1, y' == 0, a' == 0
# lpi1 is [0, 1] x [0, 1] x [1, 1]
# lpi2 is [3, 4] x [0, 1] x [1, 1]
# aggregation shouldn't need to introduce a variable along the y direction
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[0, 1], [0, 1], [1, 1]], mode)
lpi2 = lputil.from_box([[4, 5], [0, 1], [1, 1]], mode)
#a_csr = csr_matrix(np.array([[0, 0, 1], [0, 0, 0], [0, 0, 0]], dtype=float))
sqr = math.sqrt(2) / 2
agg_dirs = np.array([[1, 0, 0], [0, sqr, sqr], [0, sqr, -sqr]], dtype=float)
# box aggregation
lpi = lputil.aggregate([lpi1, lpi2], agg_dirs, mode)
# lpi1 corners
for pt in [(0, 0, 1), (0, 1, 1), (1, 0, 1), (1, 1, 1)]:
assert lputil.is_point_in_lpi(pt, lpi)
# lpi2 corners
for pt in [(4, 0, 1), (4, 1, 1), (5, 0, 1), (5, 1, 1)]:
assert lputil.is_point_in_lpi(pt, lpi)
# make sure we have new variable names
names = lpi.get_names()
expected_names = ["m0_i0", "m0_i1", "m0_i2", "m0_c0", "m0_c1", "m0_c2"]
assert names == expected_names
def test_chull_lines():
'tests aggregation of two lines in 2d using convex hull'
mode = HybridAutomaton().new_mode('mode_name')
center = [-5, -1, 7]
generator = [0.5, 0.1, 1.0]
lpi = lputil.from_zonotope(center, [generator], mode)
t1 = math.pi / 3
a_mat = np.array([[-0.3, 1, 0], [-1, -0.3, 0], [0, 0.1, 1.1]], dtype=float)
bm = expm(a_mat * t1)
lputil.set_basis_matrix(lpi, bm)
lpi_list = [lpi.clone()]
all_verts = []
verts = lpplot.get_verts(lpi)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
t2 = t1 + 0.1
bm = expm(a_mat * t2)
lputil.set_basis_matrix(lpi, bm)
lpi_list.append(lpi.clone())
verts = lpplot.get_verts(lpi)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
chull_lpi = lputil.aggregate_chull(lpi_list, mode)
#xs, ys = zip(*lpplot.get_verts(chull_lpi))
#plt.plot(xs, ys, 'r--')
#plt.show()
for vert in all_verts:
assert lputil.is_point_in_lpi(vert, chull_lpi)
def test_aggregate3():
'tests aggregation of 3 sets, inspired by the harmonic oscillator system'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[0, 1], [0, 1]], mode)
# middle set is a diamond
mat = [[1, 1], [-1, -1], [1, -1], [-1, 1]]
s = 3.5
rhs = [6+s, -(6-s), s, s]
lpi2 = lputil.from_constraints(mat, rhs, mode)
lpi3 = lputil.from_box([[5, 6], [5, 6]], mode)
lpi_list = [lpi1, lpi2, lpi3]
verts = []
for lpi in lpi_list:
verts += lpplot.get_verts(lpi)
#xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'k-')
random.seed(0)
for _ in range(10):
random_mat = np.random.rand(2, 2)
agg_dirs = lputil.reorthogonalize_matrix(random_mat, 2)
lpi = lputil.aggregate(lpi_list, agg_dirs, mode)
#xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'r--')
for vert in verts:
assert lputil.is_point_in_lpi(vert, lpi)
def test_chull_drivetrain():
'convex hull aggregation debugging from drivetrain system'
mode = HybridAutomaton().new_mode('mode_name')
center = [-0.0432, -11, 0, 30, 0, 30, 360, -0.0013, 30, -0.0013, 30, 0, 1]
generator = [0.0056, 4.67, 0, 10, 0, 10, 120, 0.0006, 10, 0.0006, 10, 0, 0]
lpi = lputil.from_zonotope(center, [generator], mode)
# neg_angle init dynamics
a_mat = np.array([ \
[0, 0, 0, 0, 0, 0, 0.0833333333333333, 0, -1, 0, 0, 0, 0], \
[13828.8888888889, -26.6666666666667, 60, 60, 0, 0, -5, -60, 0, 0, 0, 0, 116.666666666667], \
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -5], \
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], \
[0, 0, 0, 0, -714.285714285714, -0.04, 0, 0, 0, 714.285714285714, 0, 0, 0], \
[-2777.77777777778, 3.33333333333333, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -83.3333333333333], \
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], \
[100, 0, 0, 0, 0, 0, 0, -1000, -0.01, 1000, 0, 0, 3], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], \
[0, 0, 0, 0, 1000, 0, 0, 1000, 0, -2000, -0.01, 0, 0], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
], dtype=float)
# plot dimensions
xdim = 0
ydim = 1
step = 5.0E-2
t1 = 0
bm = expm(a_mat * t1)
lputil.set_basis_matrix(lpi, bm)
lpi_list = [lpi.clone()]
all_verts = []
verts = lpplot.get_verts(lpi, xdim=xdim, ydim=ydim)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
t2 = t1 + step
bm = expm(a_mat * t2)
lputil.set_basis_matrix(lpi, bm)
lpi_list.append(lpi.clone())
verts = lpplot.get_verts(lpi, xdim=xdim, ydim=ydim)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
chull_lpi = lputil.aggregate_chull(lpi_list, mode)
plot_vecs = lpplot.make_plot_vecs(num_angles=256, offset=0.01)
verts = lpplot.get_verts(chull_lpi, xdim=xdim, ydim=ydim, plot_vecs=plot_vecs)
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'r--')
#plt.show()
for vert in all_verts:
assert lputil.is_point_in_lpi(vert, chull_lpi)
def test_agg_ha():
'test aggregation with the harmonic oscillator dynamics'
ha = HybridAutomaton('Deaggregation Example')
m1 = ha.new_mode('green')
m1.set_dynamics([[0, 1], [-1, 0]])
m2 = ha.new_mode('cyan')
m2.set_dynamics([[0, 0, 0], [0, 0, -2], [0, 0, 0]])
t1 = ha.new_transition(m1, m2)
t1.set_guard_true()
reset_mat = [[1, 0], [0, 1], [0, 0]]
t1.set_reset(reset_mat, [[0], [0], [1]], [[1], [-1]], [1, -1]) # create 3rd variable with a0 = 1
mode = ha.modes['green']
init_lpi = lputil.from_box([(-5, -4), (-0.5, 0.5)], mode)
init_list = [StateSet(init_lpi, mode)]
step = math.pi/4
settings = HylaaSettings(step, 2*step)
settings.process_urgent_guards = True
settings.plot.plot_mode = PlotSettings.PLOT_NONE
settings.stdout = HylaaSettings.STDOUT_DEBUG
core = Core(ha, settings)
core.setup(init_list)
core.do_step() # pop
#xs, ys = zip(*core.cur_state.verts(core.plotman))
#plt.plot(xs, ys, 'k-')
core.do_step() # 0
#xs, ys = zip(*core.cur_state.verts(core.plotman))
#plt.plot(xs, ys, 'k-')
core.do_step() # 1
#xs, ys = zip(*core.cur_state.verts(core.plotman))
#plt.plot(xs, ys, 'k-')
core.do_step() # 2
assert len(core.aggdag.waiting_list) > 1
#for state in core.waiting_list:
# xs, ys = zip(*state.verts(core.plotman))
# plt.plot(xs, ys, 'k-')
core.do_step() # pop
assert not core.aggdag.waiting_list
lpi = core.aggdag.get_cur_state().lpi
# 3 constraints from basis matrix
# 2 aggregation directions from premode arnoldi, +1 from null space
# + 2 more aggregation directions from box (3rd is omited since it's exactly the same as null space direction)
#print(lpi)
#xs, ys = zip(*core.cur_state.verts(core.plotman))
#plt.plot(xs, ys, 'r--')
#plt.show()
assert lpi.get_num_rows() == 3 + 2 * (5)
assert lputil.is_point_in_lpi((-5, 2, 1), lpi)