def test_aggregate_self():
'''
test aggregation on an identical set.
'''
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[-2, -1], [-10, 20], [100, 200]], mode)
lpi2 = lputil.from_box([[-2, -1], [-10, 20], [100, 200]], mode)
agg_dirs = np.identity(3)
# box aggregation
lpi = lputil.aggregate([lpi1, lpi2], agg_dirs, mode)
assert lpi.is_feasible()
verts = lpplot.get_verts(lpi, xdim=0, ydim=1)
assert_verts_is_box(verts, [[-2, -1], [-10, 20]])
verts = lpplot.get_verts(lpi, xdim=0, ydim=2)
assert_verts_is_box(verts, [[-2, -1], [100, 200]])
# make sure no extra variables in lp
names = lpi.get_names()
expected_names = ["m0_i0", "m0_i1", "m0_i2", "m0_c0", "m0_c1", "m0_c2"]
assert names == expected_names
assert lpi.get_num_rows() == 3 + 3*2
def test_box_aggregate2():
'tests box 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)
agg_dirs = np.identity(2)
# box aggregation
lpi = lputil.aggregate([lpi1, lpi2], agg_dirs, mode)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [[0, 2], [0, 2]])
# test setting basis matrix after aggregation
lputil.set_basis_matrix(lpi, np.identity(2))
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [[0, 2], [0, 2]])
lputil.set_basis_matrix(lpi, -1 * np.identity(2))
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [[-2, 0], [-2, 0]])
def test_box_aggregate3():
'tests box aggregation with 3 boxes'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[-2, -1], [-0.5, 0.5]], mode)
lpi2 = lpi1.clone()
lpi3 = lpi1.clone()
basis2 = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi2, basis2)
basis3 = np.array([[-1, 0], [0, -1]], dtype=float)
lputil.set_basis_matrix(lpi3, basis3)
plot_vecs = lpplot.make_plot_vecs(256, offset=0.1) # use an offset to prevent LP dir from being aligned with axis
# bounds for lpi1 should be [[-2, -1], [-0.5, 0.5]]
verts = lpplot.get_verts(lpi1, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[-2, -1], [-0.5, 0.5]])
# bounds for lpi2 should be [[-0.5, 0.5], [1, 2]]
verts = lpplot.get_verts(lpi2, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[-0.5, 0.5], [1, 2]])
# bounds for lpi3 should be [[2, 1], [-0.5, 0.5]]
verts = lpplot.get_verts(lpi3, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[2, 1], [-0.5, 0.5]])
# box aggregation, bounds should be [[-2, 2], [-0.5, 2]]
agg_dirs = np.identity(2)
lpi = lputil.aggregate([lpi1, lpi2, lpi3], agg_dirs, mode)
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[-2, 2], [-0.5, 2]])
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_add_init_constraint():
'tests add_init_constraint on the harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
# update basis matrix
basis_mat = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis_mat)
# minimize y should give 4.0
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.0) < 1e-6
# add constraint: y >= 4.5
direction = np.array([0, -1], dtype=float)
new_row = lputil.add_init_constraint(lpi, direction, -4.5)
assert new_row == 6, "new constraint should have been added in row index 6"
# minimize y should give 4.5
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.5) < 1e-6
# check verts()
verts = lpplot.get_verts(lpi)
assert len(verts) == 5
assert [0.0, 5.0] in verts
assert [1.0, 5.0] in verts
assert [0.0, 4.5] in verts
assert [1.0, 4.5] in verts
assert verts[0] == verts[-1]
def test_verts():
'tests verts'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [(-5, -4), (0, 1)])
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_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_replace_init_constraint():
'tests try_replace_init_constraint on the harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
# update basis matrix
basis_mat = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis_mat)
# minimize y should give 4.0
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.0) < 1e-6
# add constraint: y >= 4.5
direction = np.array([0, -1], dtype=float)
row_index = lputil.add_init_constraint(lpi, direction, -4.5)
assert lpi.get_rhs()[-1] == -4.5
# minimize y should give 4.5
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.5) < 1e-6
assert lpi.get_num_rows() == 7
# try to replace constraint y >= 4.6 (should be stronger than 4.5)
row_index, is_stronger = lputil.try_replace_init_constraint(
lpi, row_index, direction, -4.6)
assert is_stronger
assert row_index == 6
assert lpi.get_num_rows() == 7
assert lpi.get_rhs()[row_index] == -4.6
# try to replace constraint x <= 0.9 (should be incomparable)
xdir = np.array([1, 0], dtype=float)
row_index, is_stronger = lputil.try_replace_init_constraint(
lpi, row_index, xdir, 0.9)
assert not is_stronger
assert lpi.get_num_rows() == 8
assert lpi.get_rhs()[row_index] == 0.9
# check verts()
verts = lpplot.get_verts(lpi)
assert len(verts) == 5
assert [0.0, 5.0] in verts
assert [0.9, 5.0] in verts
assert [0.0, 4.6] in verts
assert [0.9, 4.6] in verts
assert verts[0] == verts[-1]
def test_scale():
'tests scale'
lpi = lputil.from_box([[4, 5], [-1, 1]],
HybridAutomaton().new_mode('mode_name'))
lputil.scale_with_bm(lpi, 2.0)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(8, 10), (-2, 2)])
def test_bloat():
'tests bloat'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
lputil.bloat(lpi, 0.5)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(-5.5, -3.5), (-0.5, 1.5)])
def test_minkowski_sum_box():
'tests minkowski_sum with 2 box sets'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[-1, 1], [-2, 2]], mode)
lpi2 = lputil.from_box([[-.1, .1], [-.2, .2]], mode)
lpi = lputil.minkowski_sum([lpi1, lpi2], mode)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(-1.1, 1.1), (-2.2, 2.2)])
def test_init_triangle():
'tests initialization from a non-box initial set of states'
# x + y < 1, x > 0, y > 0
constraints_mat = [[1, 1], [-1, 0], [0, -1]]
constraints_rhs = [1, 0, 0]
lpi = lputil.from_constraints(constraints_mat, constraints_rhs,
HybridAutomaton().new_mode('mode_name'))
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0], \
[0, 1, 0, -1], \
[1, 1, 0, 0], \
[-1, 0, 0, 0], \
[0, -1, 0, 0]], dtype=float)
expected_vec = np.array([0, 0, 1, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up]
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1"]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
# check verts
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert len(verts) == 4
assert [0., 1.] in verts
assert [0., 0.] in verts
assert [1., 0] in verts
assert verts[0] == verts[-1]
def verts(self, plotman, subplot=0):
'get the vertices for plotting this state set, wraps around so rv[0] == rv[-1]'
Timers.tic('verts')
if self._verts is None:
self._verts = [None] * plotman.num_subplots
if self._verts[subplot] is None:
min_time = self.cur_steps_since_start[
0] * plotman.core.settings.step_size
max_time = self.cur_steps_since_start[
1] * plotman.core.settings.step_size
time_interval = (min_time, max_time)
if not self.assigned_plot_dim:
self.assigned_plot_dim = True
self.xdim = []
self.ydim = []
for i in range(plotman.num_subplots):
self.xdim.append(plotman.settings.xdim_dir[i])
self.ydim.append(plotman.settings.ydim_dir[i])
if isinstance(self.xdim[i], dict):
assert self.mode.name in self.xdim[
i], "mode {} not in xdim plot direction dict".format(
self.mode.name)
self.xdim[i] = self.xdim[i][self.mode.name]
if isinstance(self.ydim[i], dict):
assert self.mode.name in self.ydim[
i], "mode {} not in ydim plot direction dict".format(
self.mode.name)
self.ydim[i] = self.ydim[i][self.mode.name]
self._verts[subplot] = lpplot.get_verts(self.lpi, xdim=self.xdim[subplot], ydim=self.ydim[subplot], \
plot_vecs=plotman.plot_vec_list[subplot], cur_time=time_interval)
assert self._verts[subplot] is not None, "verts() was unsat"
Timers.toc('verts')
return self._verts[subplot]
def test_minkowski_box_diamond():
'tests minkowski_sum of a box and a diamond'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[-1, 1], [-1, 1]], mode)
# -1 <= x + y <= 1
# -1 <= x - y <= 1
constraints_mat = [[1, 1], [-1, -1], [1, -1], [-1, 1]]
constraints_rhs = [1, 1, 1, 1]
lpi2 = lputil.from_constraints(constraints_mat, constraints_rhs, mode)
#verts = lpplot.get_verts(lpi1)
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'r--')
#verts = lpplot.get_verts(lpi2)
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'b--')
lpi = lputil.minkowski_sum([lpi1, lpi2], mode)
#verts = lpplot.get_verts(lpi)
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k:')
#plt.show()
verts = lpplot.get_verts(lpi)
assert len(verts) == 9 # octogon + wrap
expected = [(-2, 1), (-2, -1), (-1, -2), (1, -2), (2, -1), (2, 1), (1, 2),
(-1, 2)]
for pt in expected:
assert pair_almost_in(pt, verts), f"{pt} not found in verts: {verts}"
def test_from_input_constraints():
'test making an lpi set from input constraints'
mode = HybridAutomaton().new_mode('mode_name')
b_mat = [[1], [0]]
b_constraints = [[1], [-1]]
b_rhs = [0.2, 0.2]
# result should have two vertices, at (-0.2, 0) and (0.2, 0)
lpi = lputil.from_input_constraints(b_mat, b_constraints, b_rhs, mode)
print(lpi)
assert lpi.get_num_cols() == 5
assert lpi.cur_vars_offset == 3
verts = lpplot.get_verts(lpi)
assert len(verts) == 3 # 2 + wrap
expected = [(-0.2, 0), (0.2, 0)]
for pt in expected:
assert pair_almost_in(pt, verts), f"{pt} not found in verts: {verts}"
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_inputs_reset():
'test a system with both inputs and a reset'
# 2-d system with one input
# x' = x, y' = u, u \in [1, 1]
# x0 = 1, y0 = 0
# inv1: y <= 2.5
# guard: y >= 2.5
# reset: x := 1, y += 2 [should go from (e^3, 3.0) -> (1, 5.0)]
# mode2:
# x' = 2x, y' = Bu, u \in [1, 2], B = 2
# (1, 5.0) -> (e^2, [7, 9]) -> (e^4, [9, 13])
# mode2 -> error y >= 13
ha = HybridAutomaton()
m1 = ha.new_mode('m1')
m1.set_dynamics([[1, 0], [0, 0]])
m1.set_inputs([[0], [1]], [[1], [-1]], [1, -1], allow_constants=True)
m1.set_invariant([[0, 1]], [2.5])
m2 = ha.new_mode('m2')
m2.set_dynamics([[2, 0], [0, 0]])
m2.set_inputs([[0], [2]], [[1], [-1]], [2, -1])
error = ha.new_mode('error')
t1 = ha.new_transition(m1, m2)
t1.set_guard([[0, -1]], [-2.5]) # y >= 2.5
reset_mat = [[0, 0], [0, 1]]
min_mat = np.identity(2)
min_cons = [[1, 0], [-1, 0], [0, 1], [0, -1]]
min_rhs = [1, -1, 2, -2]
t1.set_reset(reset_mat, min_mat, min_cons, min_rhs)
t2 = ha.new_transition(m2, error)
t2.set_guard([0, -1], [-13]) # y >= 13
init_box = [[1, 1], [0, 0]]
lpi = lputil.from_box(init_box, m1)
settings = HylaaSettings(1.0, 10.0)
settings.stdout = HylaaSettings.STDOUT_VERBOSE
settings.plot.store_plot_result = True
settings.plot.plot_mode = PlotSettings.PLOT_NONE
core = Core(ha, settings)
init_list = [StateSet(lpi, m1)]
core.setup(init_list)
core.do_step() # pop
core.do_step() # continuous_post() to time 1
lpi = core.result.last_cur_state.lpi
assert lpi.get_names() == ['m0_i0', 'm0_i1', 'm0_c0', 'm0_c1', 'm0_ti0', 'm0_ti1', 'm0_I0']
assert_verts_is_box(lpplot.get_verts(lpi), [[math.exp(1), math.exp(1)], [1, 1]])
core.do_step() # continuous_post() to time 2
assert_verts_is_box(lpplot.get_verts(core.result.last_cur_state.lpi), [[math.exp(2), math.exp(2)], [2, 2]])
core.do_step() # continuous_post() to time 3
assert_verts_is_box(lpplot.get_verts(core.result.last_cur_state.lpi), [[math.exp(3), math.exp(3)], [3, 3]])
core.do_step() # trim to invariant
assert core.aggdag.get_cur_state() is None
assert len(core.aggdag.waiting_list) == 1
core.run_to_completion()
result = core.result
# reset: x := 1, y += 2 [should go from (e^3, 3.0) -> (1, 5.0)]
# (1, 5.0) -> (e^2, [7, 9]) -> (e^4, [9, 13])
polys2 = [obj[0] for obj in result.plot_data.mode_to_obj_list[0]['m2']]
assert_verts_is_box(polys2[0], [[1, 1], [5, 5]])
assert_verts_is_box(polys2[1], [[math.exp(2), math.exp(2)], [7, 9]])
assert_verts_is_box(polys2[2], [[math.exp(4), math.exp(4)], [9, 13]])
assert len(polys2) == 3
# check counterexamples
assert len(result.counterexample) == 2
c1 = result.counterexample[0]
assert c1.mode == m1
assert c1.outgoing_transition == t1
assert np.allclose(c1.start, [1, 0])
assert np.allclose(c1.end, [math.exp(3), 3])
assert len(c1.reset_minkowski_vars) == 2
assert abs(c1.reset_minkowski_vars[0] - 1) < 1e-9
assert abs(c1.reset_minkowski_vars[1] - 2) < 1e-9
assert len(c1.inputs) == 3
for i in c1.inputs:
assert len(i) == 1
assert abs(i[0] - 1) < 1e-9
c2 = result.counterexample[1]
assert c2.mode == m2
assert c2.outgoing_transition == t2
assert np.allclose(c2.start, [1, 5])
assert np.allclose(c2.end, [math.exp(4), 13])
assert not c2.reset_minkowski_vars
assert len(c2.inputs) == 2
for i in c2.inputs:
assert len(i) == 1
assert abs(i[0] - 2) < 1e-9
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_reset_minkowski():
'''tests reset with a minkowski sum term and a new variable
pre reset we have x = [-5, -4], y = [0, 1]
post reset we have x = [-15, -14] (-10), y = [0, 1], t' = [0, 5]
reset_matrix is [[1, 0], [0, 1], [0, 0]]
minkowski_csr is [[1, 0], [0, 0], [0, 1]]
minkowski_constraints_csr is [[1, 0], [-1, 0], [0, 1], [0, -1]]
minkowski_constraints_rhs is [-10, 10, 5, 0]
'''
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
reset_csr = csr_matrix([[1, 0], [0, 1], [0, 0]], dtype=float)
mode_id = 1
transition_id = 13
minkowski_csr = csr_matrix([[1, 0], [0, 0], [0, 1]], dtype=float)
constraints_csr = csr_matrix([[1, 0], [-1, 0], [0, 1], [0, -1]],
dtype=float)
constraints_rhs = np.array([-10, 10, 5, 0], dtype=float)
lputil.add_reset_variables(lpi, mode_id, transition_id, reset_csr=reset_csr, minkowski_csr=minkowski_csr, \
minkowski_constraints_csr=constraints_csr, minkowski_constraints_rhs=constraints_rhs)
assert lpi.dims == 3
# basis matrix should be at 9, 6
assert lpi.basis_mat_pos == (9, 6)
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1", "reset0", "reset1", "m1_i0_t13", "m1_i1", "m1_i2", \
"m1_c0", "m1_c1", "m1_c2"]
assert lpi.get_names() == expected_names
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, xdim=0, ydim=1, plot_vecs=plot_vecs)
assert len(verts) == 5
assert [-15.0, 0.] in verts
assert [-15.0, 1.] in verts
assert [-14.0, 1.] in verts
assert [-14.0, 0.] in verts
assert verts[0] == verts[-1]
verts = lpplot.get_verts(lpi,
xdim=2,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0.0)
assert len(verts) == 3
assert [0, 0.] in verts
assert [5, 0.] in verts
assert verts[0] == verts[-1]
lputil.set_basis_matrix(lpi, 3 * np.identity(3))
verts = lpplot.get_verts(lpi,
xdim=2,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0.0)
assert len(verts) == 3
assert [0, 0.] in verts
assert [15, 0.] in verts
assert verts[0] == verts[-1]
def test_approx_lgg_inputs():
'test lgg approximation model with inputs'
# simple dynamics, x' = 1, y' = 0 + u, a' = 0, u in [0.1, 0.2]
# step size (tau) 0.02
# after one step, the input effect size should by tau*V \oplus beta*B
# we'll manually assign beta to be 0.02, in order to be able to check that the constraints are correct
# A norm is 1
tau = 0.05
a_matrix = [[0, 0, 1], [0, 0, 0], [0, 0, 0]]
b_mat = [[0], [1], [0]]
b_constraints = [[1], [-1]]
b_rhs = [0.2, -0.1]
mode = HybridAutomaton().new_mode('mode')
mode.set_dynamics(a_matrix)
mode.set_inputs(b_mat, b_constraints, b_rhs)
init_lpi = lputil.from_box([[0, 0], [0, 0], [1, 1]], mode)
assert lputil.compute_radius_inf(init_lpi) == 1
ss = StateSet(init_lpi, mode)
mode.init_time_elapse(tau)
assert_verts_equals(lpplot.get_verts(ss.lpi), [(0, 0)])
ss.apply_approx_model(HylaaSettings.APPROX_LGG)
assert np.linalg.norm(a_matrix, ord=np.inf) == 1.0
v_set = lputil.from_input_constraints(mode.b_csr, mode.u_constraints_csc,
mode.u_constraints_rhs, mode)
assert lputil.compute_radius_inf(v_set) == 0.2
alpha = (math.exp(tau) - 1 - tau) * (1 + 0.2)
assert_verts_equals(lpplot.get_verts(ss.lpi), \
[(0, 0), (tau-alpha, 0.2*tau + alpha), (tau+alpha, 0.2*tau+alpha), (tau+alpha, 0.1*tau-alpha)])
# note: c gets bloated by alpha as well
assert (ss.lpi.minimize(
[0, 0, -1])[ss.lpi.cur_vars_offset + 2]) - (1 + alpha) < 1e-9
assert (ss.lpi.minimize(
[0, 0, 1])[ss.lpi.cur_vars_offset + 2]) - (1 - alpha) < 1e-9
# c is actually growing, starting at (1,1) at x=0 and going to [1-alpha, 1+alpha] at x=tau
assert_verts_equals(lpplot.get_verts(ss.lpi, xdim=0, ydim=2), \
[(0, 1), (tau-alpha, 1+alpha), (tau+alpha, 1+alpha), (tau+alpha, 1-alpha), (tau-alpha, 1-alpha)])
# ready to start
ss.step()
beta = (math.exp(tau) - 1 - tau) * 0.2
# note: c gets bloated as well! so now it's [1-epsilon, 1+epsilon], where epsilon=alpha
# so x will grow by [tau * (1 - alpha), tau * (1 + alpha)]
expected = [(tau + beta, -beta + tau * 0.1), \
(tau - beta, -beta + tau * 0.1), \
(tau - beta, beta + tau * 0.2), \
((tau - alpha) + tau * (1 - alpha) - beta, 2*0.2*tau + alpha + beta), \
((tau + alpha) + tau * (1 + alpha) + beta, 2*0.2*tau+alpha + beta), \
((tau + alpha) + tau * (1 + alpha) + beta, 2*0.1*tau-alpha - beta)]
#xs, ys = zip(*expected)
#plt.plot([x for x in xs] + [xs[0]], [y for y in ys] + [ys[0]], 'r-') # expected is red
verts = lpplot.get_verts(ss.lpi)
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-+') # computed is black
#plt.show()
assert_verts_equals(verts, expected)
# one more step should work without errors
ss.step()
def test_box_inputs():
'tests from_box with a simple input effects matrix'
# x' = Ax + Bu
# A = 0
# B = [[1, 0], [0, 2]]
# u1 and u2 are bounded between [1, 10]
# (init) step 0: [0, 1] x [0, 1]
# step 1: [1, 11] x [2, 21]
# step 2: [2, 21] x [4, 41]
mode = HybridAutomaton().new_mode('mode_name')
mode.set_dynamics(np.zeros((2, 2)))
mode.set_inputs([[1, 0], [0, 2]], [[1, 0], [-1, 0], [0, 1], [0, -1]],
[10, -1, 10, -1])
init_box = [[0, 1], [0, 1]]
lpi = lputil.from_box(init_box, mode)
assert lpi.basis_mat_pos == (0, 0)
assert lpi.dims == 2
assert lpi.cur_vars_offset == 2
assert lpi.input_effects_offsets == (
6, 4) # row 6, column 4 for total input effects offsets
# step 0
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 1, 0], \
[0, 1, 0, -1, 0, 1], \
[-1, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0], \
[0, 0, 0, 0, -1, 0], \
[0, 0, 0, 0, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 0, 1, 0, 1, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx]
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1", "m0_ti0", "m0_ti1"]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, init_box)
# do step 1
mode.init_time_elapse(1.0)
basis_mat, input_mat = mode.time_elapse.get_basis_matrix(1)
lputil.set_basis_matrix(lpi, basis_mat)
lputil.add_input_effects_matrix(lpi, input_mat, mode)
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 1, 0, 0, 0], \
[0, 1, 0, -1, 0, 1, 0, 0], \
[-1, 0, 0, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0, 0, 0], \
[0, 0, 0, 0, -1, 0, 1, 0], \
[0, 0, 0, 0, 0, -1, 0, 2], \
[0, 0, 0, 0, 0, 0, 1, 0], \
[0, 0, 0, 0, 0, 0, -1, 0], \
[0, 0, 0, 0, 0, 0, 0, 1], \
[0, 0, 0, 0, 0, 0, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 0, 1, 0, 1, 0, 0, 10, -1, 10, -1],
dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx, up, up, up, up]
expected_names = [
"m0_i0", "m0_i1", "m0_c0", "m0_c1", "m0_ti0", "m0_ti1", "m0_I0",
"m0_I1"
]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(1, 11), (2, 21)])
# do step 2
basis_mat, input_mat = mode.time_elapse.get_basis_matrix(2)
lputil.set_basis_matrix(lpi, basis_mat)
lputil.add_input_effects_matrix(lpi, input_mat, mode)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(2, 21), (4, 41)])
def test_add_reset_inputs():
'tests add_reset_variables'
mode = HybridAutomaton().new_mode('mode_name')
lpi = lputil.from_box([[-5, -4], [0, 1]], mode)
reset_csr = csr_matrix(2 * np.identity(2))
mode_id = 1
transition_id = 13
lputil.add_reset_variables(lpi,
mode_id,
transition_id,
reset_csr=reset_csr,
successor_has_inputs=True)
assert lpi.dims == 2
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 0, 0, 0, 0, 0, 0], \
[0, 1, 0, -1, 0, 0, 0, 0, 0, 0], \
[-1, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, 0, 2, 0, -1, 0, 0, 0, 0, 0], \
[0, 0, 0, 2, 0, -1, 0, 0, 0, 0], \
[0, 0, 0, 0, 1, 0, -1, 0, 1, 0], \
[0, 0, 0, 0, 0, 1, 0, -1, 0, 1], \
[0, 0, 0, 0, 0, 0, 0, 0, -1, 0], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 5, -4, 0, 1, 0, 0, 0, 0, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx, fx, fx, fx, fx]
expected_names = [
"m0_i0", "m0_i1", "m0_c0", "m0_c1", "m1_i0_t13", "m1_i1", "m1_c0",
"m1_c1", "m1_ti0", "m1_ti1"
]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
assert lpi.basis_mat_pos == (8, 4)
assert lpi.input_effects_offsets == (10, 8)
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert len(verts) == 5
assert [-10.0, 0.] in verts
assert [-10.0, 2.] in verts
assert [-8.0, 2.] in verts
assert [-8.0, 0.] in verts
assert verts[0] == verts[-1]
def test_reset_less_dims():
'''tests a reset to a mode with less dimensions
project onto just the y variable multiplied by 0.5
'''
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
assert lpi.dims == 2
reset_csr = csr_matrix(np.array([[0, 0.5]], dtype=float))
mode_id = 1
transition_id = 13
lputil.add_reset_variables(lpi,
mode_id,
transition_id,
reset_csr=reset_csr)
assert lpi.dims == 1
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 0, 0], \
[0, 1, 0, -1, 0, 0], \
[-1, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0], \
[0, 0, 0, 0.5, -1, 0], \
[0, 0, 0, 0, 1, -1]], dtype=float)
expected_vec = np.array([0, 0, 5, -4, 0, 1, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx]
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1", "m1_i0_t13", "m1_c0"]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
assert lpi.basis_mat_pos == (7, 4)
assert lpi.dims == 1
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi,
xdim=0,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0)
assert len(verts) == 3
assert [0.5, 0] in verts
assert [0, 0] in verts
assert verts[0] == verts[-1]
# update the basis matrix
basis = np.array([[2]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
verts = lpplot.get_verts(lpi,
xdim=0,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0)
assert len(verts) == 3
assert [1.0, 0] in verts
assert [0, 0] in verts
assert verts[0] == verts[-1]