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]