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 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():
'''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=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(spec, limit, uncertain_inputs):
'''make the hybrid automaton and return it'''
ha = LinearHybridAutomaton()
mode = ha.new_mode('mode')
dynamics = loadmat('build.mat')
a_matrix = dynamics['A']
n = a_matrix.shape[0]
b_matrix = csc_matrix(dynamics['B'])
if uncertain_inputs:
mode.set_dynamics(csr_matrix(a_matrix))
# 0 <= u1 <= 0.1
bounds_list = [(0.8, 1.0)]
_, u_mat, u_rhs, u_range_tuples = bounds_list_to_init(bounds_list)
mode.set_inputs(b_matrix, u_mat, u_rhs, u_range_tuples)
else:
# add the input as a state variable
big_a_mat = np.zeros((n + 1, n + 1))
big_a_mat[0:n, 0:n] = a_matrix.toarray()
big_a_mat[0:n, n:n + 1] = b_matrix.toarray()
a_matrix = big_a_mat
mode.set_dynamics(csr_matrix(big_a_mat))
error = ha.new_mode('error')
y1 = dynamics['C'][0]
if not uncertain_inputs:
new_y1 = np.zeros((1, n + 1))
new_y1[0, 0:n] = y1
y1 = new_y1
output_space = csr_matrix(y1)
mode.set_output_space(output_space)
trans1 = ha.new_transition(mode, error)
mat = csr_matrix(([-1], [0], [0, 1]), dtype=float, shape=(1, 1))
rhs = np.array([-limit], dtype=float) # safe
trans1.set_guard(mat, rhs) # y3 >= limit
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", "t"]
loc1 = ha.new_mode('loc1')
loc1.a_matrix = np.array([[-0.1, -1, 0, 0], [1, -0.1, 0, 0],
[0, 0, -0.15, 0], [0, 0, 0, 0]])
loc1.c_vector = np.array([0, 0, 0, 1], dtype=float)
error = ha.new_mode('_error')
error.is_error = True
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.50))
# usafe_set_constraint_list.append(LinearConstraint([-1, 0, 0], -0.25))
usafe_set_constraint_list.append(LinearConstraint([1, 0, 0, 0], 0.8))
usafe_set_constraint_list.append(LinearConstraint([-1, 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, 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 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):
# 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.96065997, 0.1947354], [-0.1947354, 0.96065997]])
loc1.c_vector = np.array([0.39340481, -0.03933961], 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], 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=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=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):
# 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.1, 1], [-1, -0.1]])
# loc1.a_matrix = np.array([[0, 1], [-1, 0]])
loc1.c_vector = np.array([1, 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([0.0, -1.0], -4))
# 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, 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():
'''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 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):
# 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 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(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=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 = ["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 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)
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(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(limit):
'''make the hybrid automaton and return it'''
ha = LinearHybridAutomaton()
mode = ha.new_mode('mode')
dynamics = loadmat('iss.mat')
a_matrix = dynamics['A']
# a is a csc_matrix
col_ptr = [n for n in a_matrix.indptr]
rows = [n for n in a_matrix.indices]
data = [n for n in a_matrix.data]
b_matrix = dynamics['B']
num_inputs = b_matrix.shape[1]
for u in xrange(num_inputs):
rows += [n for n in b_matrix[:, u].indices]
data += [n for n in b_matrix[:, u].data]
col_ptr.append(len(data))
combined_mat = csc_matrix((data, rows, col_ptr), \
shape=(a_matrix.shape[0] + num_inputs, a_matrix.shape[1] + num_inputs))
mode.set_dynamics(csr_matrix(combined_mat))
error = ha.new_mode('error')
y3 = dynamics['C'][2]
col_ptr = [n for n in y3.indptr] + num_inputs * [y3.data.shape[0]]
y3 = csc_matrix((y3.data, y3.indices, col_ptr), shape=(1, y3.shape[1] + num_inputs))
output_space = csr_matrix(y3)
#print "y3.data = {}, y3.indices = {}, y3.col_ptr = {}".format(y3.data, y3.indices, y3.col_ptr)
mode.set_output_space(output_space)
trans1 = ha.new_transition(mode, error)
mat = csr_matrix(([1], [0], [0, 1]), dtype=float, shape=(1, 1))
rhs = np.array([-limit], dtype=float) # safe
trans1.set_guard(mat, rhs) # y3 <= -limit
trans2 = ha.new_transition(mode, error)
mat = csr_matrix(([-1], [0], [0, 1]), dtype=float, shape=(1, 1))
rhs = np.array([-limit], dtype=float) # safe
trans2.set_guard(mat, rhs) # y3 >= limit
return ha
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
