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_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_set_basis_matrix(): 'tests lputil set_basis_matrix on harmonic oscillator example' lpi = lputil.from_box([[-5, -4], [0, 1]], HybridAutomaton().new_mode('mode_name')) basis = np.array([[0, 1], [-1, 0]], dtype=float) lputil.set_basis_matrix(lpi, basis) assert np.allclose(lputil.get_basis_matrix(lpi), basis) mat, vec = lpi.get_full_constraints(), lpi.get_rhs() expected_mat = np.array([\ [0, 1, -1, 0], \ [-1, 0, 0, -1], \ [-1, 0, 0, 0], \ [1, 0, 0, 0], \ [0, -1, 0, 0], \ [0, 1, 0, 0]], dtype=float) expected_vec = np.array([0, 0, 5, -4, 0, 1], dtype=float) assert np.allclose(vec, expected_vec) assert np.allclose(mat.toarray(), expected_mat)
def test_get_box_center(): 'test get_box_center' lpi = lputil.from_box([[-5, -4], [0, 1]], HybridAutomaton().new_mode('mode_name')) pt = lputil.get_box_center(lpi) assert len(pt) == 2 assert abs(pt[0] - (-4.5)) < 1e-4 assert abs(pt[1] - (0.5)) < 1e-4 basis = np.array([[0, 1], [-1, 0]], dtype=float) lputil.set_basis_matrix(lpi, basis) pt = lputil.get_box_center(lpi) assert len(pt) == 2 assert abs(pt[0] - (0.5)) < 1e-4 assert abs(pt[1] - (4.5)) < 1e-4 # try it rotated 1/4 around the circle a_mat = np.array([[0, 1], [-1, 0]], dtype=float) bm = expm(a_mat * math.pi / 4) lputil.set_basis_matrix(lpi, bm) expected = np.dot(bm, np.array([[-4.5], [0.5]], dtype=float)) pt = lputil.get_box_center(lpi) assert len(pt) == 2 assert abs(pt[0] - expected[0][0]) < 1e-4 assert abs(pt[1] - expected[1][0]) < 1e-4
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_chull_one_step_inputs(): 'test convex hull with one-step lpi for a system with inputs (bug where current vars was not set correctly)' mode = HybridAutomaton().new_mode('mode_name') step_size = math.pi/4 a_mat = np.array([[0, 1], [-1, 0]], dtype=float) b_mat = [[1], [0]] b_constraints = [[1], [-1]] b_rhs = [0.2, 0.2] mode.set_dynamics(a_mat) mode.set_inputs(b_mat, b_constraints, b_rhs) mode.init_time_elapse(step_size) box = [[-5, -4], [0.0, 1.0]] lpi = lputil.from_box(box, mode) lpi_one_step = lpi.clone() bm, ie_mat = mode.time_elapse.get_basis_matrix(1) lputil.set_basis_matrix(lpi_one_step, bm) lputil.add_input_effects_matrix(lpi_one_step, ie_mat, mode) lpi_list = [lpi, lpi_one_step] chull_lpi = lputil.aggregate_chull(lpi_list, mode) # 2 current vars and 2 total input effect vars, so expected to be 4 from the end assert chull_lpi.cur_vars_offset == chull_lpi.get_num_cols() - 4, "cur_vars in wrong place"
def step(self, step_in_mode=None): '''update the star based on values from a new simulation time instant the default is to advance by one step, otherwise step_in_mode can force going to a specific step number ''' Timers.tic("step") # self.basis_matrix, input_effects_matrix = self.mode.time_elapse.get_basis_matrix(0) # print(self.basis_matrix, input_effects_matrix, self.lpi) if step_in_mode is None: step_in_mode = self.cur_step_in_mode + 1 num_steps = step_in_mode - self.cur_step_in_mode # we can't do negative steps because we add input effects in the lpi for each step assert num_steps >= 0, "step() called with negative num steps (mode: " + \ f"{self.mode.name}, cur_step_in_mode: {self.cur_step_in_mode}, requested_step: {step_in_mode})" if num_steps > 0: Timers.tic('get_bm') self.basis_matrix, input_effects_matrix = self.mode.time_elapse.get_basis_matrix(step_in_mode) Timers.toc('get_bm') Timers.tic('set_bm') lputil.set_basis_matrix(self.lpi, self.basis_matrix) Timers.toc('set_bm') if input_effects_matrix is not None: Timers.tic('input effects matrix') # if we're doing multiple steps here we need to get each step's input effects matrix for step in range(self.cur_step_in_mode + 1, step_in_mode): _, ie_mat = self.mode.time_elapse.get_basis_matrix(step) self.input_effects_list.append(ie_mat) lputil.add_input_effects_matrix(self.lpi, ie_mat, self.mode, self.lgg_beta) # add the input effects matrix for the final step (computed before with basis matrix) self.input_effects_list.append(input_effects_matrix) lputil.add_input_effects_matrix(self.lpi, input_effects_matrix, self.mode, self.lgg_beta) Timers.toc('input effects matrix') # print(f".ss lp columns = {self.lpi.get_num_cols()}") self.cur_step_in_mode += num_steps self.cur_steps_since_start[0] += num_steps self.cur_steps_since_start[1] += num_steps self._verts = None # cached vertices no longer valid # print(self.basis_matrix, input_effects_matrix, self.lpi) # if P1_lpi is not None: # if isinstance(P1_lpi, LpInstance): # check_poly_contain_efficient_w_proj(P1_lpi=P1_lpi, P2_lpi=self.lpi) # elif isinstance(P1_lpi, list): # P1_box = P1_lpi # check_poly_contain_brute_force(P1_box=P1_box, P2_lpi=self.lpi) # else: # print("Provide a correct type for P1") Timers.toc("step")
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_get_basis_matrix(): 'tests lputil get_basis_matrix on harmonic oscillator example' lpi = lputil.from_box([[-5, -4], [0, 1]], HybridAutomaton().new_mode('mode_name')) basis = np.array([[0, 1], [-1, 0]], dtype=float) lputil.set_basis_matrix(lpi, basis) mat = lputil.get_basis_matrix(lpi) assert np.allclose(mat, basis)
def test_check_intersection(): 'tests check_intersection on the harmonic oscillator example' lpi = lputil.from_box([[-5, -4], [0, 1]], HybridAutomaton().new_mode('mode_name')) # check if initially y >= 4.5 is possible (should be false) direction = np.array([0, -1], dtype=float) lc = LinearConstraint(direction, -4.5) assert not lputil.check_intersection(lpi, lc) # after basis matrix update basis = np.array([[0, 1], [-1, 0]], dtype=float) lputil.set_basis_matrix(lpi, basis) # now check if y >= 4.5 is possible (should be true) assert lputil.check_intersection(lpi, lc)
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 apply_approx_chull(self): ''' apply convex hull approximation model ''' lpi_one_step = self.lpi.clone() Timers.tic('get_bm') bm, ie_mat = self.mode.time_elapse.get_basis_matrix(1) Timers.toc('get_bm') Timers.tic('set_bm') lputil.set_basis_matrix(lpi_one_step, bm) Timers.toc('set_bm') if ie_mat is not None: Timers.tic('input effects matrix') lputil.add_input_effects_matrix(lpi_one_step, ie_mat, self.mode) Timers.toc('input effects matrix') lpi_list = [self.lpi, lpi_one_step] self.lpi = lputil.aggregate_chull(lpi_list, self.mode)
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]
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 apply_approx_lgg(self): ''' apply lgg approximation model from equation (2) in Lemma 1 of: "Reachability analysis of linear systems using support functions", Le Guernic, C., Girard, A., Nonlinear Analysis: Hybrid Systems, 2010 ''' has_inputs = self.mode.b_csr is not None # use infinity norm a_norm = sp.sparse.linalg.norm(self.mode.a_csr, ord=np.inf) lpi_one_step = self.lpi.clone() Timers.tic('get_bm') bm, _ = self.mode.time_elapse.get_basis_matrix(1) Timers.toc('get_bm') Timers.tic('set_bm') lputil.set_basis_matrix(lpi_one_step, bm) Timers.toc('set_bm') mode = self.mode tau = mode.time_elapse.step_size r_x0 = lputil.compute_radius_inf(self.lpi) if has_inputs: v_set = lputil.from_input_constraints(mode.b_csr, mode.u_constraints_csc, mode.u_constraints_rhs, mode) r_v = lputil.compute_radius_inf(v_set) # minkowski sum with tau * V # V is the input set, V = B U lputil.scale_with_bm(v_set, tau) msum = lputil.minkowski_sum([lpi_one_step, v_set], mode) else: r_v = 0 msum = lpi_one_step tol = 1e-7 if a_norm < tol: print( f"Warning: norm of dynamics A matrix was small ({a_norm}), using alpha = 0 and " + "beta = 0 in LGG approximation model") alpha = 0 else: alpha = (math.exp(tau * a_norm) - 1 - tau * a_norm) * (r_x0 + r_v / a_norm) #print(f".ss alpha={alpha}") if alpha != 0: # bloat by alpha (minkowski sum) lputil.bloat(msum, alpha) self.lpi = lputil.aggregate_chull([self.lpi, msum], mode) if a_norm > tol and has_inputs: # precompute beta as well self.lgg_beta = (math.exp(tau * a_norm) - 1 - tau * a_norm) * (r_v / a_norm) #print(f".ss beta={self.lgg_beta}") assert self.lgg_beta > tol, f"lgg approx model beta was too close to zero: {self.lgg_beta}" assert self.lgg_beta < 1e5, f"lgg approx model beta was too large (use a smaller step): {self.lgg_beta}" self.lpi.set_minimize_direction([-1] * self.lpi.dims) self.mode.time_elapse.use_lgg_approx()
def test_add_reset_variables(): 'tests add_reset_variables' lpi = lputil.from_box([[-5, -4], [0, 1]], HybridAutomaton().new_mode('mode_name')) 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) 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, 1, 0, -1, 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, 0, 0, 0, 0, 0, 0], \ [0, 0, 2, 0, -1, 0, 0, 0], \ [0, 0, 0, 2, 0, -1, 0, 0], \ [0, 0, 0, 0, 1, 0, -1, 0], \ [0, 0, 0, 0, 0, 1, 0, -1]], dtype=float) expected_vec = np.array([0, 0, 5, -4, 0, 1, 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] expected_names = [ "m0_i0", "m0_i1", "m0_c0", "m0_c1", "m1_i0_t13", "m1_i1", "m1_c0", "m1_c1" ] 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) 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] # update the basis matrix to rotate quarter circle basis = np.array([[0, 1], [-1, 0]], dtype=float) lputil.set_basis_matrix(lpi, basis) verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs) assert len(verts) == 5 assert [0.0, 10.] in verts assert [0.0, 8.] in verts assert [2.0, 8.] in verts assert [2.0, 10.] in verts assert verts[0] == verts[-1]
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)])