def test_eat_self(self):
'test star.eat_star() on an equal star (should not change constraints)'
hr = HyperRectangle([(1, 2), (2, 2), (-6, -3)])
star = init_hr_to_star(make_settings(), hr, TestStar.loc)
star2 = init_hr_to_star(make_settings(), hr, TestStar.loc)
star.eat_star(star2)
tol = 1e-6
# constraints should be the same as before
a_mat = make_star_a_mat(star)
a_mat2 = make_star_a_mat(star2)
b_vec = make_star_b_vec(star)
b_vec2 = make_star_b_vec(star2)
self.assertEqual(len(b_vec), len(b_vec2))
for row_index in xrange(len(b_vec)):
assert_allclose(a_mat[row_index],
a_mat2[row_index],
rtol=tol,
atol=tol)
self.assertAlmostEqual(b_vec[row_index], b_vec2[row_index])
self.check_stars_equal(star, star2)
def test_eat_other(self):
'test star.eat_star() on a non-equal star: [1,2] eating [2, 3] results in [1, 3]'
star = init_hr_to_star(make_settings(), HyperRectangle([(1, 2)]),
TestStar.loc)
star2 = init_hr_to_star(make_settings(), HyperRectangle([(2, 3)]),
TestStar.loc)
expected = init_hr_to_star(make_settings(), HyperRectangle([(1, 3)]),
TestStar.loc)
star.eat_star(star2)
self.check_stars_equal(star, expected)
def test_eat_other_2d(self):
'test star.eat_star() on a 2-d non-equal star'
star = init_hr_to_star(make_settings(),
HyperRectangle([(-5, -2), (10, 20)]),
TestStar.loc)
star2 = init_hr_to_star(make_settings(),
HyperRectangle([(20, 34), (-3, -2)]),
TestStar.loc)
expected = init_hr_to_star(make_settings(),
HyperRectangle([(-5, 34), (-3, 20)]),
TestStar.loc)
star.eat_star(star2)
self.check_stars_equal(star, expected)
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, 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 test_eat_rotated_unit(self):
'test star.eat_star() on a rotated unit-square eating the unit square'
size = math.sqrt(2.0) / 2.0
basis = [[size, size], [size, -size]]
a_mat = [[1., 0.], [-1., 0.], [0., 1.], [0., -1.]]
b_vec = [1, 1, 1, 1]
center = [0, 0]
star = make_star(center, basis, a_mat, b_vec)
star2 = init_hr_to_star(make_settings(),
HyperRectangle([(-1, 1), (-1, 1)]),
TestStar.loc)
size = 1
basis = [[size, size], [size, -size]]
a_mat = [[1., 0.], [-1., 0.], [0., 1.], [0., -1.]]
b_vec = [1, 1, 1, 1]
center = [0, 0]
expected = make_star(center, basis, a_mat, b_vec)
star.eat_star(star2)
self.check_stars_equal(star, expected)
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 load_waiting_list(self, init_list):
'''convert the init list into self.waiting_list'''
assert len(init_list) > 0, "initial list length is 0"
for mode, shape in init_list:
assert isinstance(mode, LinearAutomatonMode)
if isinstance(shape, HyperRectangle):
star = init_hr_to_star(self.settings, shape, mode)
elif isinstance(shape, list):
assert len(
shape
) > 0, "initial constraints in mode '{}' was empty list".format(
mode.name)
assert isinstance(shape[0], LinearConstraint)
star = init_constraints_to_star(self.settings, shape, mode)
else:
raise RuntimeError(
"Unsupported initial state type '{}': {}".format(
type(shape), shape))
still_feasible, _ = star.trim_to_invariant()
if still_feasible:
self.waiting_list.add_deaggregated(star)
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, 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=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, 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, 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 test_square_eat_parallelogram(self):
'test square star eating parallelogram star'
star = init_hr_to_star(make_settings(),
HyperRectangle([(0, 1), (0, 1)]), TestStar.loc)
basis = [[1, 0], [1, 1]]
a_mat = [[1., 0.], [-1, 0.], [0., 1.], [0., -1.]]
b_vec = [1, 0, 1, 0]
center = [0, 0]
par = make_star(center, basis, a_mat, b_vec)
expected = init_hr_to_star(make_settings(),
HyperRectangle([(0, 2), (0, 1)]),
TestStar.loc)
star.eat_star(par)
self.check_stars_equal(star, expected)
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", "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 __init__(self, settings, ha, init, usafe_set_constraint_list, error_stars, reachable_stars=None):
self.error_stars = error_stars
self.reachable_stars = reachable_stars
self.usafe_set_constraint_list = usafe_set_constraint_list
## Compute the init star
self.init_star = init_hr_to_star(settings, init[0][1], init[0][0])
self.num_dims = len(ha.variables)
self.step = settings.step
self.num_steps = settings.num_steps
self.ha = ha
def test_init_constraints(self):
'test hylaa initialization using constraints'
hr = HyperRectangle([(-1, 4), (-1, 1)])
star = init_hr_to_star(make_settings(), hr, TestStar.loc)
# now try a list of LinearConstraints
constraints = [
LinearConstraint([-1, 0], 1),
LinearConstraint([1, 0], 4),
LinearConstraint([0, -1], 1),
LinearConstraint([0, 1], 1)
]
star2 = init_constraints_to_star(make_settings(), constraints,
TestStar.loc)
self.check_stars_equal(star, star2)
def test_boundaries_optimized(self):
'test finding the boundaries in an optimized fashion'
Star.init_plot_vecs(2, TestStar.plot_settings)
hr = HyperRectangle([
(0, 1),
(0, 1),
])
star = init_hr_to_star(make_settings(), hr, TestStar.loc)
start_op = LpInstance.total_optimizations()
verts = star.verts()
num_op = LpInstance.total_optimizations() - start_op
self.assertEqual(len(verts), 5)
self.assertLess(num_op, 100)
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(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_offset_dif_basis(self):
'test a case an offset square eating another one, with non-standard basis'
basis = [[0, 1], [1, 0]]
a_mat = [[0, 1], [-1, 0], [1, 0], [0, -1]]
b_vec = [4, 1, 1, -2]
center = [0, 0]
a2 = make_star(center, basis, a_mat, b_vec)
basis = [[0, 1], [1, 0]]
a_mat = [[0, 1], [-1, 0], [1, 0], [0, -1]]
b_vec = [1, 1, 1, 1]
center = [0, 0]
b = make_star(center, basis, a_mat, b_vec)
a2.eat_star(b)
expected = init_hr_to_star(make_settings(),
HyperRectangle([(-1, 4), (-1, 1)]),
TestStar.loc)
self.check_stars_equal(a2, expected)
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, 1]])
loc1.a_matrix = np.array([[0.9851, -0.0988, 0, 0], [0.0988, 0.9851, 0, 0],
[0, 0, 0.9851, 0], [0, 0, 0, 1]])
loc1.c_vector = np.array([0, 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 test_hr_to_star(self):
'test hr converstion to a star'
hr = HyperRectangle([(1, 2), (2, 2), (-6, -3)])
star = init_hr_to_star(make_settings(), hr, TestStar.loc)
self.assertTrue(isinstance(star, Star))
expected_basis = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
assert_allclose(star.basis_matrix,
expected_basis,
rtol=1e-10,
atol=1e-10)
expected_a_mat = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0],
[0, -1, 0], [0, 0, 1], [0, 0, -1]])
a_mat = make_star_a_mat(star)
assert_allclose(a_mat, expected_a_mat, rtol=1e-10, atol=1e-10)
expected_b = np.array([2, -1, 2, -2, -3, 6])
b_vec = make_star_b_vec(star)
assert_allclose(b_vec, expected_b, rtol=1e-10, atol=1e-10)
