def define_ha(settings, args): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' usafe_r = args[0] ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4"] dim = len(ha.variables) loc1 = ha.new_mode('loc1') loc1.c_vector = np.array([0, 0, 0, 0], dtype=float) print(loc1.c_vector.shape) a_matrix = np.array( [[1, 0, 0.1, 0], [0, 1, 0, 0.1], [0, 0, 0.8870, 0.0089], [0, 0, 0.0089, 0.8870]], dtype=float) b_matrix = np.array([[0, 0], [0, 0], [1, 0], [0, 1]], dtype=float) # Q = 1 * np.eye(dim) Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 100]], dtype=float) u_dim = len(b_matrix[0]) R = np.eye(u_dim) (X, L, G) = care(a_matrix, b_matrix, Q, R) control_f = open("./control_vals.txt", "a") control_f.write("Q: " + str(Q) + "\n") control_f.write("P: " + str(X) + "\n") control_f.write("K: " + str(G) + "\n") control_f.write("PBK: " + str(np.matmul(X, np.matmul(b_matrix, G))) + "\n") control_f.write("PA: " + str(np.matmul(X, a_matrix)) + "\n") control_f.write("A'P: " + str(np.matmul(a_matrix.T, X)) + "\n") control_f.close() k_matrix = np.array(G) loc1.a_matrix = a_matrix - np.matmul(b_matrix, k_matrix) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0, 0.0, 0.0], 2.0)) usafe_set_constraint_list.append(LinearConstraint([1.0, 0.0, 0.0, 0.0], -1.0)) usafe_set_constraint_list.append(LinearConstraint([0.0, -1.0, 0.0, 0.0], 3)) usafe_set_constraint_list.append(LinearConstraint([0.0, 1.0, 0.0, 0.0], -2)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "y"] loc1 = ha.new_mode('loc1') loc1.a_matrix = np.array([[-0.1, 1], [-1, -0.1]]) # loc1.a_matrix = np.array([[0, 1], [-1, 0]]) loc1.c_vector = np.array([2, 0], dtype=float) # loc1.set_dynamics(a_matrix, c_vector) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0], 2)) usafe_set_constraint_list.append(LinearConstraint([1.0, 0.0], 2)) usafe_set_constraint_list.append(LinearConstraint([0.0, -1.0], -1)) usafe_set_constraint_list.append(LinearConstraint([0.0, 1.0], 6)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton(name="Oscillating Particle") ha.variables = ["x", "y", "z"] loc1 = ha.new_mode('loc1') loc1.a_matrix = np.array([[0.722468865032875, -0.523371053120237, 0], [0.785056579680355, 0.696300312376864, 0], [0, 0, 0.930530895811206]]) loc1.c_vector = np.array([0, 0.1, 0.03], dtype=float) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([0, -1, 0], -0.5)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3"] # loc1 = ha.new_mode('loc1') loc1.a_matrix = np.array([[-0.1, -1, 0], [1, -0.1, 0], [0, 0, -0.15]]) loc1.c_vector = np.array([0, 0, 0], dtype=float) # print(a_bk_matrix) error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: # exp 1 usafe_set_constraint_list.append(LinearConstraint([1, 0, 0], -0.46)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans = ha.new_transition(loc1, error) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4"] # loc1 = ha.new_mode('loc1') a_matrix = np.array([[1, 0, 0.1, 0], [0, 1, 0, 0.1], [0, 0, 0.8870, 0.0089], [0, 0, 0.0089, 0.8870]], dtype=float) # exp 1 b_matrix = np.array([[1, 0], [0, 0], [1, 0], [0, 1]], dtype=float) print(a_matrix, b_matrix) R_mult_factor = 0.1 Q_matrix = np.eye(len(a_matrix[0]), dtype=float) u_dim = len(b_matrix[0]) R_matrix = R_mult_factor * np.eye(u_dim) print(a_matrix, b_matrix, Q_matrix, R_matrix) k_matrix = get_input(a_matrix, b_matrix, Q_matrix, R_matrix) print(k_matrix) a_bk_matrix = a_matrix - np.matmul(b_matrix, k_matrix) loc1.a_matrix = a_bk_matrix loc1.c_vector = np.array([0.0, 0.0, 0.0, 0.0], dtype=float) # print(a_bk_matrix) error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: # exp 1 # significant diff (10 sec) across equivalent/non-equ runs for p_intersect without reverse # usafe_set_constraint_list.append(LinearConstraint([1.0, 0.0, 0.0, 0.0], -4.8)) # exp 2 # significant diff (13-15 sec) across equivalent/non-equ runs for p_intersect without reverse # usafe_set_constraint_list.append(LinearConstraint([0.0, 0.0, 1.0, 0.0], -5.0)) # exp 3 usafe_set_constraint_list.append( LinearConstraint([1.0, 0.0, 0.0, 0.0], -5.2)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans = ha.new_transition(loc1, error) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton(name="Damped Oscillator") ha.variables = ["x", "y"] loc1 = ha.new_mode('loc1') loc1.a_matrix = np.array([[0.960659959352277, 0.194735414472060], [-0.194735414472060, 0.960659959352277]]) loc1.c_vector = np.array([0.196702394590923, -0.019669801188631], dtype=float) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([0.0, -1.0], -4)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4", "x5", "x6", "x7", "x8", "t"] # input variable order: [u1, u2] Model = ha.new_mode('Model') a_matrix = np.array([[0, 1, 0, 0, 0, 0, 0, 0, 0], [0, -1.0865, 8487.2, 0, 0, 0, 0, 0, 0], [-2592.1, -21.119, -698.91, -141399, 0, 0, 0, 0, 0], [1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, -1.0865, 8487.2, 0, 0], [0, 0, 0, 0, -2592.1, -21.119, -698.91, -141399, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) c_vector = np.array([0, 0, 0, 0, 0, 0, 0, 0, 1], dtype=float) Model.set_dynamics(a_matrix, c_vector) # u1 >= 0.16 # u1 <= 0.3 # u2 >= 0.2 # u2 <= 0.4 u_constraints_a = np.array([[-1, -0], [1, 0], [-0, -1], [0, 1]], dtype=float) u_constraints_b = np.array([-0.16, 0.3, -0.2, 0.4], dtype=float) b_matrix = np.array([[0, 0], [0, 0], [0, 0], [-1, 0], [0, 0], [0, 0], [0, 0], [0, -1], [0, 0]], dtype=float) Model.set_inputs(u_constraints_a, u_constraints_b, b_matrix) _error = ha.new_mode('_error') _error.is_error = True trans = ha.new_transition(Model, _error) trans.condition_list.append(LinearConstraint([0, 0, 0, 0, -1, 0, 0, 0, 0], -0.001501)) # 0.45 <= x5 return ha
def test_eat_star_bs2_1d(self): 'test 1d eat-star derived from example-2 of the ball_string system' array = np.array settings = HylaaSettings(0.01, 2.0) ha = LinearHybridAutomaton('Test Automaton') ha.variables = ["x"] mode = ha.new_mode('test_mode') center = array([0.]) basis_matrix = array([[1.0]], dtype=float) # -alpha <= 1.0 ----> -1.0 <= alpha # 2 * alpha <= -1 ----> alpha <= -0.5 cur_star = Star(settings, center, basis_matrix, [\ LinearConstraint(array([-1.]), 1.0), \ LinearConstraint(array([2.0]), -1.0),\ ], \ None, mode) # -0.7 <= alpha <= -0.6 new_star = Star(settings, center, basis_matrix, [\ LinearConstraint(array([-1.]), 0.7), \ LinearConstraint(array([1.]), -0.6), \ ], \ None, mode) cur_star.eat_star(new_star) # should be unchanged: -0.0 <= alpha and 2 * alpha <= 1.0 self.assertAlmostEqual(cur_star.constraint_list[0].value, 1.0) self.assertAlmostEqual(cur_star.constraint_list[1].value, -1.0)
def define_ha(): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "y"] # input variable order: [u1, u2] loc1 = ha.new_mode('loc1') a_matrix = np.array([ \ [0, 1], \ [-1, 0], \ ], dtype=float) c_vector = np.array([0, 0], dtype=float) loc1.set_dynamics(a_matrix, c_vector) # -0.5 <= u1 # u1 <= 0.5 # -0.5 <= u2 # u2 <= 0.5 u_constraints_a = np.array([[-1, 0], [1, 0], [0, -1], [0, 1]], dtype=float) u_constraints_b = np.array([0.5, 0.5, 0.5, 0.5], dtype=float) b_matrix = np.array([[1, 0], [0, 1]], dtype=float) loc1.set_inputs(u_constraints_a, u_constraints_b, b_matrix) return ha
def test_eat_star_bs1_1d(self): 'test 1d eat-star with example-1 derived from the ball_string system' settings = HylaaSettings(0.01, 2.0) array = np.array ha = LinearHybridAutomaton('Test Automaton') ha.variables = ["x"] mode = ha.new_mode('test_mode') center = array([0.]) # x = 0.5 * alpha (2x = alpha) # 0 <= alpha <= 1.0 -> (0 <= x <= 0.5) cur_star = Star(settings, center, array([[0.5]]), [ \ LinearConstraint(array([1.]), 1.0), \ LinearConstraint(array([-1.]), 0.0)], \ None, mode) # 2.0 <= alpha <= 3.0 -> (1.0 <= x <= 1.5) new_star = Star(settings, center, array([[0.5]]), [ \ LinearConstraint(array([1.]), 3.0), \ LinearConstraint(array([-1.]), -2.0)], \ None, mode) cur_star.eat_star(new_star) # should be 0.0 <= alpha <= 3.0 self.assertAlmostEqual(cur_star.constraint_list[0].value, 3.0) self.assertAlmostEqual(cur_star.constraint_list[1].value, 0.0)
def define_ha(settings, args): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' usafe_r = args[0] x_ref = args[1] step_inputs = args[2] k_matrix = args[3] a_matrices = args[4] b_matrices = args[5] dim = len(b_matrices[0]) ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4", "t"] locations = [] n_locations = len(step_inputs) for idx in range(n_locations): loc_name = 'loc' + str(idx) loc = ha.new_mode(loc_name) a_matrix = a_matrices[idx] b_matrix = b_matrices[idx] b_k_matrix = np.matmul(b_matrix, k_matrix) loc.a_matrix = a_matrix + b_k_matrix c_vector = -np.matmul(b_k_matrix, x_ref[idx]) c_vector = c_vector + np.matmul(b_matrix, step_inputs[idx]) c_vector[dim - 1] = step_size # print(c_vector) loc.c_vector = c_vector loc.inv_list.append( LinearConstraint([0.0, 0.0, 0.0, 0.0, 1.0], step_size * idx)) # t <= 0.1 locations.append(loc) for idx in range(n_locations - 1): trans = ha.new_transition(locations[idx], locations[idx + 1]) trans.condition_list.append( LinearConstraint([-0.0, -0.0, 0.0, 0.0, -1.0], -step_size * idx)) # t >= 0.1 error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append( LinearConstraint([1.0, 0.0, 0.0, 0.0, 0.0], -3.5)) # usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0, 0.0, 0.0, 0.0], -3.5)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for idx in range(n_locations - 1): trans = ha.new_transition(locations[idx], error) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def make_debug_mode(): 'make an AutomatonMode object' ha = LinearHybridAutomaton('Test Automaton') ha.variables = ["x", "y"] loc = ha.new_mode('test_mode') return loc
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2"] # loc1 = ha.new_mode('loc1') # exp 1 and 2 a_matrix = np.array([[0.983498664120250, 0.101548195541291], [-0.013528375561473, 0.935610369333783]], dtype=float) # exp 1 b_matrix = np.array([[0.0], [0.0]], dtype=float) # # exp2 # b_matrix = np.array([[1], [1]], dtype=float) print(a_matrix, b_matrix) R_mult_factor = 0.2 Q_matrix = np.eye(len(a_matrix[0]), dtype=float) u_dim = len(b_matrix[0]) R_matrix = R_mult_factor * np.eye(u_dim) print(a_matrix, b_matrix, Q_matrix, R_matrix) k_matrix = get_input(a_matrix, b_matrix, Q_matrix, R_matrix) print(k_matrix) # a_bk_matrix = a_matrix_ext - np.matmul(b_matrix_ext, k_matrix) a_bk_matrix = a_matrix - np.matmul(b_matrix, k_matrix) loc1.a_matrix = a_bk_matrix loc1.c_vector = np.array([0.0, 0.0], dtype=float) error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: # exp 1 usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0], -2.0)) # usafe_set_constraint_list.append(LinearConstraint([0.0, 1.0], -0.85)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans = ha.new_transition(loc1, error) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, args): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' usafe_r = args[0] if len(args) > 2: x_ref = args[1] step_inputs = args[2] ha = LinearHybridAutomaton() ha.variables = ["x", "y", "t"] a_matrix = np.array([[2, -1, 0], [1, 0, 0], [0, 0, 1]]) b_matrix = np.array([[2], [0], [0]], dtype=float) k_matrix = np.array([[-0.85559968, 0.46109066, 0]], dtype=float) locations = [] n_locations = len(step_inputs) for idx in range(n_locations): loc_name = 'loc' + str(idx) loc = ha.new_mode(loc_name) b_k_matrix = np.matmul(b_matrix, k_matrix) loc.a_matrix = a_matrix + b_k_matrix c_vector = -np.matmul(b_k_matrix, x_ref[idx]) c_vector = c_vector + np.matmul(b_matrix, step_inputs[idx]) c_vector[len(ha.variables) - 1] = step_size # print(c_vector) loc.c_vector = c_vector # loc.c_vector = np.array([step_inputs[idx], step_inputs[idx], 1], dtype=float) loc.inv_list.append(LinearConstraint([0.0, 0.0, 1.0], step_size * idx)) # t <= 0.1 locations.append(loc) for idx in range(n_locations - 1): trans = ha.new_transition(locations[idx], locations[idx + 1]) trans.condition_list.append( LinearConstraint([-0.0, -0.0, -1.0], -step_size * idx)) # t >= 0.1 error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(locations[0], error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0, 0.0], 1)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4"] # loc1 = ha.new_mode('loc1') a_matrix = np.array( [[1, 0, 0, 0], [0, 2, 0.5, 0], [0, 0, 1, 0], [0, 0, 0, 0.5]], dtype=float) # exp 1 b_matrix = np.array( [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 3, 0], [0, 2, 0, 1]], dtype=float) print(a_matrix, b_matrix) R_mult_factor = 0.1 Q_matrix = np.eye(len(a_matrix[0]), dtype=float) u_dim = len(b_matrix[0]) R_matrix = R_mult_factor * np.eye(u_dim) print(a_matrix, b_matrix, Q_matrix, R_matrix) k_matrix = get_input(a_matrix, b_matrix, Q_matrix, R_matrix) print(k_matrix) a_bk_matrix = a_matrix - np.matmul(b_matrix, k_matrix) loc1.a_matrix = a_bk_matrix loc1.c_vector = np.array([0.0, 0.0, 0.0, 0.0], dtype=float) # print(a_bk_matrix) error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: # exp 1 # usafe_set_constraint_list.append(LinearConstraint([0.0, 0.0, 0.0, 1.0], -2.9)) # exp 2 usafe_set_constraint_list.append( LinearConstraint([0.0, 0.0, 0.0, 1.0], -3.42)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans = ha.new_transition(loc1, error) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4", "x5", "x6", "x7", "x8"] loc1 = ha.new_mode('loc1') a_matrix = np.array([[1, 0.1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0.1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0.1, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0.1], [0, 0, 0, 0, 0, 0, 0, 1]], dtype=float) loc1.c_vector = np.array([0, 0, 0, 0, 0, 0, 0, 0], dtype=float) b_matrix = np.array([[0, 0, 0, 0], [0.1, 0, 0, 0], [0, 0, 0, 0], [0.1, -0.1, 0, 0], [0, 0, 0, 0], [0, 0.1, -0.1, 0], [0, 0, 0, 0], [0, 0, 0.1, -0.1]], dtype=float) k_matrix = np.array([[401.0025, 40.0500, 1.0000, 0.0499, 0.0025, 0.0001, 0.0000, 0.0000], [400.0025, 40.0001, -401.0000, -40.0499, 1.0000, 0.0499, 0.0025, 0.0001], [400.0000, 40.0000, -400.0025, -40.0001, -401.0000, -40.0499, 1.0000, 0.0499], [400.0000, 40.0000, -400.0000, -40.0000, -400.0025, -40.0001, -401.0025, -40.0500]], dtype=float) loc1.a_matrix = a_matrix - np.matmul(b_matrix, k_matrix) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0, 0.0, 0.0], 2)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "y"] loc1 = ha.new_mode('loc1') a_matrix = np.array([[-0.1, 1], [-1, -0.1]]) c_vector = np.array([0, 0], dtype=float) loc1.set_dynamics(a_matrix, c_vector) return ha
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "y", "t"] step_inputs = [[-0.6835318568657612], [-0.4274739688077575], [0.4047028719575525], [-1.7181706660550653], [0.6195838154872904], [-0.981255069072019], [1.0521099187388827], [1.1240072822724865], [2.0], [1.0260517738498387]] a_matrix = np.array([[0, 2, 0], [1, 0, 0], [0, 0, 1]]) b_matrix = np.array([[1], [1], [0]], dtype=float) locations = [] n_locations = len(step_inputs) for idx in range(n_locations): loc_name = 'loc' + str(idx) loc = ha.new_mode(loc_name) loc.a_matrix = a_matrix c_vector = np.matmul(b_matrix, step_inputs[idx]) c_vector[len(ha.variables) - 1] = step_size print(c_vector) loc.c_vector = c_vector # loc.c_vector = np.array([step_inputs[idx], step_inputs[idx], 1], dtype=float) loc.inv_list.append(LinearConstraint([0.0, 0.0, 1.0], step_size * idx)) locations.append(loc) for idx in range(n_locations - 1): trans = ha.new_transition(locations[idx], locations[idx + 1]) trans.condition_list.append( LinearConstraint([-0.0, -0.0, -1.0], -idx * step_size)) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(locations[0], error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0, 0.0], 1)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "y"] loc1 = ha.new_mode('loc1') a_matrix = np.array([[0.98965, 1.4747e-08], [7.4506e-09, 0]], dtype=float) loc1.c_vector = np.array([0, 0], dtype=float) b_matrix = np.array([[16], [0]], dtype=float) Q = np.array([[1, 0], [0, 1]], dtype=float) u_dim = len(b_matrix[0]) R = np.eye(u_dim) (X, L, G) = care(a_matrix, b_matrix, Q, R) control_f = open("./control_vals.txt", "a") control_f.write("Q: "+str(Q)+"\n") control_f.write("P: "+str(X)+"\n") control_f.write("K: "+str(G)+"\n") control_f.write("PBK: "+str(np.matmul(X, np.matmul(b_matrix, G)))+"\n") control_f.write("PA: "+str(np.matmul(X, a_matrix))+"\n") control_f.write("A'P: "+str(np.matmul(a_matrix.T, X))+"\n") control_f.close() k_matrix = np.array(G) a_bk_matrix = a_matrix - np.matmul(b_matrix, k_matrix) loc1.a_matrix = a_bk_matrix error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0], -0.2)) usafe_set_constraint_list.append(LinearConstraint([0.0, 1.0], 0.0)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["temp", "t"] power = 7.0 high = 22.0 low = 18.0 c = 0.4 Tenv = 10.0 on = ha.new_mode('on') on.a_matrix = np.array([[-c, 0.0], [0.0, 0.0]], dtype=float) on.c_vector = np.array([Tenv * c + power, 1.0], dtype=float) on.inv_list.append(LinearConstraint([1.0, 0.0], high)) # temp <= high off = ha.new_mode('off') off.a_matrix = np.array([[-c, 0.0], [0.0, 0.0]], dtype=float) off.c_vector = np.array([Tenv * c, 1.0], dtype=float) off.inv_list.append(LinearConstraint([-1.0, 0.0], -low)) # temp >= low trans1_2 = ha.new_transition(on, off) trans1_2.condition_list.append(LinearConstraint([-1.0, 0.0], -high)) # temp > high trans2_1 = ha.new_transition(off, on) trans2_1.condition_list.append(LinearConstraint([1.0, 0.0], low)) # temp < low error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0], -21)) # temp >= high # usafe_set_constraint_list.append(LinearConstraint([1.0, 0.0], low)) # temp <= low else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans1_error = ha.new_transition(on, error) trans2_error = ha.new_transition(off, error) for constraint in usafe_set_constraint_list: trans1_error.condition_list.append(constraint) trans2_error.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton('Trimmed Harmonic Oscillator') ha.variables = ["x", "y"] loc1 = ha.new_mode('loc1') loc1.a_matrix = nparray([[-0.2, 1], [-1, -0.2]]) loc1.c_vector = nparray([0, 0]) inv1 = LinearConstraint([0., 1.], 4.0) # y <= 4 loc1.inv_list = [inv1] return ha
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x1", "x2", "x3", "x4"] loc1 = ha.new_mode('loc1') a_matrix = np.array([[1, 0, 0.1, 0], [0, 1, 0, 0.1], [0, 0, 0.8870, 0.0089], [0, 0, 0.0089, 0.8870]], dtype=float) loc1.c_vector = np.array([0, 0, 0, 0], dtype=float) b_matrix = np.array([[1, 0], [0, 0], [1, 0], [0, 1]], dtype=float) k_matrix = np.array([[24.2999, 2.2873, -19.7882, 0.0153], [0.0771, 46.7949, -0.0618, 4.2253]], dtype=float) loc1.a_matrix = a_matrix - np.matmul(b_matrix, k_matrix) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append( LinearConstraint([-1.0, 0.0, 0.0, 0.0], 2)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "v"] extension = ha.new_mode('extension') extension.a_matrix = np.array([[0.9951, 0.0098], [-0.9786, 0.9559]], dtype=float) extension.c_vector = np.array([-0.0005, -0.0960], dtype=float) extension.inv_list.append(LinearConstraint([1.0, 0.0], 0)) # x <= 0 freefall = ha.new_mode('freefall') freefall.a_matrix = np.array([[1.0, 0.01], [0.0, 1.0]], dtype=float) freefall.c_vector = np.array([-0.0005, -0.0981], dtype=float) freefall.inv_list.append(LinearConstraint([-1.0, 0.0], 0.0)) # 0 <= x freefall.inv_list.append(LinearConstraint([1.0, 0.0], 1.0)) # x <= 1 trans = ha.new_transition(extension, freefall) trans.condition_list.append(LinearConstraint([-0.0, -1.0], -0.0)) # v >= 0 error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: usafe_set_constraint_list.append(LinearConstraint([-1.0, 0.0], 0.5)) usafe_set_constraint_list.append(LinearConstraint([0.0, -1.0], -5.0)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans1 = ha.new_transition(extension, error) for constraint in usafe_set_constraint_list: trans1.condition_list.append(constraint) trans2 = ha.new_transition(freefall, error) for constraint in usafe_set_constraint_list: trans2.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(settings, usafe_r=None): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["e1", "e1p", "a1", "e2", "e2p", "a2", "e3", "e3p", "a3"] loc1 = ha.new_mode('loc1') loc1.a_matrix = np.array([[0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, -1, 0, 0, 0, 0, 0, 0], [1.7152555329, 3.9705119979, -4.3600526739, -0.9999330812, -1.5731541104, 0.2669165553, -0.2215507198, -0.4303855023, 0.0669078193], [0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, -1, 0, 0, 0], [0.7153224517, 2.3973578876, 0.2669165553, 1.4937048131, 3.5401264957, -4.2931448546, -1.0880831031, -1.7613009555, 0.2991352608], [0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1, 0, 0, -1], [0.493771732, 1.9669723853, 0.0669078193, 0.6271724298, 2.2092110425, 0.2991352608, 1.4593953061, 3.4633677762, -4.2704788265]], dtype=float) loc1.c_vector = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=float) error = ha.new_mode('_error') error.is_error = True usafe_set_constraint_list = [] if usafe_r is None: # exp 1 usafe_set_constraint_list.append(LinearConstraint([0, 1, 0, 0, 0, 0, 0, 0, 0], -0.37)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) trans = ha.new_transition(loc1, error) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def define_ha(): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton() ha.variables = ["x", "v"] extension = ha.new_mode('extension') extension.a_matrix = np.array([[0.0, 1.0], [-100.0, -4.0]], dtype=float) extension.c_vector = np.array([0.0, -9.81], dtype=float) extension.inv_list.append(LinearConstraint([1.0, 0.0], 0)) # x <= 0 freefall = ha.new_mode('freefall') freefall.a_matrix = np.array([[0.0, 1.0], [0.0, 0.0]], dtype=float) freefall.c_vector = np.array([0.0, -9.81], dtype=float) freefall.inv_list.append(LinearConstraint([-1.0, 0.0], 0.0)) # 0 <= x freefall.inv_list.append(LinearConstraint([1.0, 0.0], 1.0)) # x <= 1 trans = ha.new_transition(extension, freefall) trans.condition_list.append(LinearConstraint([-0.0, -1.0], -0.0)) # v >= 0 return ha
def define_ha(settings, usafe_r): # x' = Ax + Bu + c '''make the hybrid automaton and return it''' # k = -0.0025 # Unsafe # k = -1.5 # Safe ha = LinearHybridAutomaton() ha.variables = ["s", "v", "vf", "a", "t"] loc1 = ha.new_mode('loc1') # loc1.a_matrix = np.array([[0, -1, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 0], [1, -4, 3, -3, 0], [0, 0, 0, 0, 0]]) # loc1.a_matrix = np.array([[0, -1, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 0], [1, -3, 2, -3, 0], [0, 0, 0, 0, 0]]) # loc1.a_matrix = np.array([[0, -1, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 0], [1, -3, 2, -2, 0], [0, 0, 0, 0, 0]]) loc1.a_matrix = np.array([[0, -1, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 0], [1, -4, 3, -1.2, 0], [0, 0, 0, 0, 0]]) # loc1.a_matrix = np.array([[0, -1, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 0], [2, -5, 3, -4, 0], [0, 0, 0, 0, 0]]) # Not stable loc1.c_vector = np.array([0, 0, 0, -10, 1], dtype=float) error = ha.new_mode('_error') error.is_error = True trans = ha.new_transition(loc1, error) usafe_set_constraint_list = [] if usafe_r is None: # usafe_set_constraint_list.append(LinearConstraint([-1, 0, 0, 0, 0], -0)) # usafe_set_constraint_list.append(LinearConstraint([1, 0, 0, 0, 0], 2)) usafe_set_constraint_list.append(LinearConstraint([0, 1, 0, 0, 0], 15)) # usafe_set_constraint_list.append(LinearConstraint([0, -1, 0, 0, 0], -20)) else: usafe_star = init_hr_to_star(settings, usafe_r, ha.modes['_error']) for constraint in usafe_star.constraint_list: usafe_set_constraint_list.append(constraint) for constraint in usafe_set_constraint_list: trans.condition_list.append(constraint) return ha, usafe_set_constraint_list
def test_rectangular(self): '''test integration of x' = 1, y' = 2''' ha = LinearHybridAutomaton('Harmonic Oscillator') ha.variables = ["x", "y"] # x' = x loc1 = ha.new_mode('loc') loc1.a_matrix = np.array([[0, 0], [0, 0]]) loc1.c_vector = np.array([1, 2]) # x(0) = 1, y(0) = 2 init_list = [(ha.modes['loc'], HyperRectangle([(0.99, 1.01), (1.99, 2.01)]))] plot_settings = PlotSettings() plot_settings.plot_mode = PlotSettings.PLOT_NONE settings = HylaaSettings(step=0.1, max_time=1.1, plot_settings=plot_settings) settings.print_output = False engine = HylaaEngine(ha, settings) engine.load_waiting_list(init_list) # x(t) = 1 + t; y(t) = 2 + 2*t # pop from waiting_list (doesn't advance state) engine.do_step() for i in xrange(10): engine.do_step() t = 0.1 * (i + 1) star = engine.cur_state point = [1 + t, 2 + 2 * t] self.assertTrue(star.contains_point(point))
def test_exp(self): '''test integration of x' = x''' ha = LinearHybridAutomaton('Harmonic Oscillator') ha.variables = ["x"] # x' = x a_matrix = np.array([[1]], dtype=float) c_vector = np.array([0], dtype=float) loc1 = ha.new_mode('loc') loc1.set_dynamics(a_matrix, c_vector) # x(0) = 1 init_list = [(ha.modes['loc'], HyperRectangle([(0.99, 1.01)]))] plot_settings = PlotSettings() plot_settings.plot_mode = PlotSettings.PLOT_NONE settings = HylaaSettings(step=0.1, max_time=1.1, plot_settings=plot_settings) settings.print_output = False engine = HylaaEngine(ha, settings) engine.load_waiting_list(init_list) # pop from waiting_list (doesn't advance state) engine.do_step() # x(t) should be e^t for i in xrange(10): engine.do_step() t = 0.1 * (i + 1) star = engine.cur_state self.assertTrue(star.contains_point([math.exp(t)]))
def define_ha(): '''make the hybrid automaton and return it''' ha = LinearHybridAutomaton('Trimmed Harmonic Oscillator w/ successor') ha.variables = ["x", "y"] loc1 = ha.new_mode('green') loc1.a_matrix = nparray([[0, 1], [-1, 0]]) loc1.c_vector = nparray([0, 0]) inv1 = LinearConstraint([0., -1.], 0.0) # y >= 0 loc1.inv_list = [inv1] loc2 = ha.new_mode('cyan') loc2.a_matrix = nparray([[0, 0], [0, 0]]) loc2.c_vector = nparray([0, -2]) inv2 = LinearConstraint([0., -1.], 2.5) # y >= -2.5 loc2.inv_list = [inv2] loc3 = ha.new_mode('orange') loc3.a_matrix = nparray([[0, 0], [0, 0]]) loc3.c_vector = nparray([0, -2]) inv3 = LinearConstraint([0., -1.], 4.0) # y >= -4 loc3.inv_list = [inv3] guard = LinearConstraint([0., -1.], 0.0) # y >= 0 trans = ha.new_transition(loc1, loc2) trans.condition_list = [guard] guard1 = LinearConstraint([0., 1.], -0) # y <= -0 guard2 = LinearConstraint([1., 0], 0.5) # x <= 0.5 guard3 = LinearConstraint([-1., 0], 0.5) # x >= -0.5 trans = ha.new_transition(loc2, loc3) trans.condition_list = [guard1, guard2, guard3] return ha
def test_eat_simple_1d(self): ''' test star.eat_star() on a simple 1d example: star1 is [3, 5] with center 4 star2 is [8, 10] with center 8 expected result is that star1 becomes [3, 10] with center 4, so constraints become: -x <= -1 x <= 6 ''' ha = LinearHybridAutomaton('Test Automaton') ha.variables = ["x"] mode = ha.new_mode('test_mode') constraint_list = [] constraint_list.append(LinearConstraint(np.array([-1.0]), -1)) constraint_list.append(LinearConstraint(np.array([1.0]), 1)) star1 = init_constraints_to_star(make_settings(), constraint_list, mode) star1.center = np.array([4.0]) constraint_list = [] constraint_list.append(LinearConstraint(np.array([-1.0]), 0)) constraint_list.append(LinearConstraint(np.array([1.0]), 2)) star2 = init_constraints_to_star(make_settings(), constraint_list, mode) star2.center = np.array([8.0]) star1.eat_star(star2) self.assertAlmostEqual(star1.center[0], 4.0) self.assertEqual(len(star1.constraint_list), 2) self.assertAlmostEqual(star1.constraint_list[0].value, -1.0) self.assertAlmostEqual(star1.constraint_list[1].value, 6.0)