def test_box_aggregate3():
'tests box aggregation with 3 boxes'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[-2, -1], [-0.5, 0.5]], mode)
lpi2 = lpi1.clone()
lpi3 = lpi1.clone()
basis2 = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi2, basis2)
basis3 = np.array([[-1, 0], [0, -1]], dtype=float)
lputil.set_basis_matrix(lpi3, basis3)
plot_vecs = lpplot.make_plot_vecs(256, offset=0.1) # use an offset to prevent LP dir from being aligned with axis
# bounds for lpi1 should be [[-2, -1], [-0.5, 0.5]]
verts = lpplot.get_verts(lpi1, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[-2, -1], [-0.5, 0.5]])
# bounds for lpi2 should be [[-0.5, 0.5], [1, 2]]
verts = lpplot.get_verts(lpi2, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[-0.5, 0.5], [1, 2]])
# bounds for lpi3 should be [[2, 1], [-0.5, 0.5]]
verts = lpplot.get_verts(lpi3, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[2, 1], [-0.5, 0.5]])
# box aggregation, bounds should be [[-2, 2], [-0.5, 2]]
agg_dirs = np.identity(2)
lpi = lputil.aggregate([lpi1, lpi2, lpi3], agg_dirs, mode)
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [[-2, 2], [-0.5, 2]])
def test_add_init_constraint():
'tests add_init_constraint on the harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
# update basis matrix
basis_mat = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis_mat)
# minimize y should give 4.0
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.0) < 1e-6
# add constraint: y >= 4.5
direction = np.array([0, -1], dtype=float)
new_row = lputil.add_init_constraint(lpi, direction, -4.5)
assert new_row == 6, "new constraint should have been added in row index 6"
# minimize y should give 4.5
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.5) < 1e-6
# check verts()
verts = lpplot.get_verts(lpi)
assert len(verts) == 5
assert [0.0, 5.0] in verts
assert [1.0, 5.0] in verts
assert [0.0, 4.5] in verts
assert [1.0, 4.5] in verts
assert verts[0] == verts[-1]
def test_set_basis_matrix():
'tests lputil set_basis_matrix on harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
basis = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
assert np.allclose(lputil.get_basis_matrix(lpi), basis)
mat, vec = lpi.get_full_constraints(), lpi.get_rhs()
expected_mat = np.array([\
[0, 1, -1, 0], \
[-1, 0, 0, -1], \
[-1, 0, 0, 0], \
[1, 0, 0, 0], \
[0, -1, 0, 0], \
[0, 1, 0, 0]], dtype=float)
expected_vec = np.array([0, 0, 5, -4, 0, 1], dtype=float)
assert np.allclose(vec, expected_vec)
assert np.allclose(mat.toarray(), expected_mat)
def test_get_box_center():
'test get_box_center'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
pt = lputil.get_box_center(lpi)
assert len(pt) == 2
assert abs(pt[0] - (-4.5)) < 1e-4
assert abs(pt[1] - (0.5)) < 1e-4
basis = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
pt = lputil.get_box_center(lpi)
assert len(pt) == 2
assert abs(pt[0] - (0.5)) < 1e-4
assert abs(pt[1] - (4.5)) < 1e-4
# try it rotated 1/4 around the circle
a_mat = np.array([[0, 1], [-1, 0]], dtype=float)
bm = expm(a_mat * math.pi / 4)
lputil.set_basis_matrix(lpi, bm)
expected = np.dot(bm, np.array([[-4.5], [0.5]], dtype=float))
pt = lputil.get_box_center(lpi)
assert len(pt) == 2
assert abs(pt[0] - expected[0][0]) < 1e-4
assert abs(pt[1] - expected[1][0]) < 1e-4
def test_box_aggregate2():
'tests box aggregation'
mode = HybridAutomaton().new_mode('mode_name')
lpi1 = lputil.from_box([[0, 1], [0, 1]], mode)
lpi2 = lputil.from_box([[1, 2], [1, 2]], mode)
agg_dirs = np.identity(2)
# box aggregation
lpi = lputil.aggregate([lpi1, lpi2], agg_dirs, mode)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [[0, 2], [0, 2]])
# test setting basis matrix after aggregation
lputil.set_basis_matrix(lpi, np.identity(2))
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [[0, 2], [0, 2]])
lputil.set_basis_matrix(lpi, -1 * np.identity(2))
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [[-2, 0], [-2, 0]])
def test_chull_one_step_inputs():
'test convex hull with one-step lpi for a system with inputs (bug where current vars was not set correctly)'
mode = HybridAutomaton().new_mode('mode_name')
step_size = math.pi/4
a_mat = np.array([[0, 1], [-1, 0]], dtype=float)
b_mat = [[1], [0]]
b_constraints = [[1], [-1]]
b_rhs = [0.2, 0.2]
mode.set_dynamics(a_mat)
mode.set_inputs(b_mat, b_constraints, b_rhs)
mode.init_time_elapse(step_size)
box = [[-5, -4], [0.0, 1.0]]
lpi = lputil.from_box(box, mode)
lpi_one_step = lpi.clone()
bm, ie_mat = mode.time_elapse.get_basis_matrix(1)
lputil.set_basis_matrix(lpi_one_step, bm)
lputil.add_input_effects_matrix(lpi_one_step, ie_mat, mode)
lpi_list = [lpi, lpi_one_step]
chull_lpi = lputil.aggregate_chull(lpi_list, mode)
# 2 current vars and 2 total input effect vars, so expected to be 4 from the end
assert chull_lpi.cur_vars_offset == chull_lpi.get_num_cols() - 4, "cur_vars in wrong place"
def step(self, step_in_mode=None):
'''update the star based on values from a new simulation time instant
the default is to advance by one step, otherwise step_in_mode can force
going to a specific step number
'''
Timers.tic("step")
# self.basis_matrix, input_effects_matrix = self.mode.time_elapse.get_basis_matrix(0)
# print(self.basis_matrix, input_effects_matrix, self.lpi)
if step_in_mode is None:
step_in_mode = self.cur_step_in_mode + 1
num_steps = step_in_mode - self.cur_step_in_mode
# we can't do negative steps because we add input effects in the lpi for each step
assert num_steps >= 0, "step() called with negative num steps (mode: " + \
f"{self.mode.name}, cur_step_in_mode: {self.cur_step_in_mode}, requested_step: {step_in_mode})"
if num_steps > 0:
Timers.tic('get_bm')
self.basis_matrix, input_effects_matrix = self.mode.time_elapse.get_basis_matrix(step_in_mode)
Timers.toc('get_bm')
Timers.tic('set_bm')
lputil.set_basis_matrix(self.lpi, self.basis_matrix)
Timers.toc('set_bm')
if input_effects_matrix is not None:
Timers.tic('input effects matrix')
# if we're doing multiple steps here we need to get each step's input effects matrix
for step in range(self.cur_step_in_mode + 1, step_in_mode):
_, ie_mat = self.mode.time_elapse.get_basis_matrix(step)
self.input_effects_list.append(ie_mat)
lputil.add_input_effects_matrix(self.lpi, ie_mat, self.mode, self.lgg_beta)
# add the input effects matrix for the final step (computed before with basis matrix)
self.input_effects_list.append(input_effects_matrix)
lputil.add_input_effects_matrix(self.lpi, input_effects_matrix, self.mode, self.lgg_beta)
Timers.toc('input effects matrix')
# print(f".ss lp columns = {self.lpi.get_num_cols()}")
self.cur_step_in_mode += num_steps
self.cur_steps_since_start[0] += num_steps
self.cur_steps_since_start[1] += num_steps
self._verts = None # cached vertices no longer valid
# print(self.basis_matrix, input_effects_matrix, self.lpi)
# if P1_lpi is not None:
# if isinstance(P1_lpi, LpInstance):
# check_poly_contain_efficient_w_proj(P1_lpi=P1_lpi, P2_lpi=self.lpi)
# elif isinstance(P1_lpi, list):
# P1_box = P1_lpi
# check_poly_contain_brute_force(P1_box=P1_box, P2_lpi=self.lpi)
# else:
# print("Provide a correct type for P1")
Timers.toc("step")
def test_replace_init_constraint():
'tests try_replace_init_constraint on the harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
# update basis matrix
basis_mat = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis_mat)
# minimize y should give 4.0
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.0) < 1e-6
# add constraint: y >= 4.5
direction = np.array([0, -1], dtype=float)
row_index = lputil.add_init_constraint(lpi, direction, -4.5)
assert lpi.get_rhs()[-1] == -4.5
# minimize y should give 4.5
miny = lpi.minimize([0, 1], columns=[lpi.cur_vars_offset + 1])[0]
assert abs(miny - 4.5) < 1e-6
assert lpi.get_num_rows() == 7
# try to replace constraint y >= 4.6 (should be stronger than 4.5)
row_index, is_stronger = lputil.try_replace_init_constraint(
lpi, row_index, direction, -4.6)
assert is_stronger
assert row_index == 6
assert lpi.get_num_rows() == 7
assert lpi.get_rhs()[row_index] == -4.6
# try to replace constraint x <= 0.9 (should be incomparable)
xdir = np.array([1, 0], dtype=float)
row_index, is_stronger = lputil.try_replace_init_constraint(
lpi, row_index, xdir, 0.9)
assert not is_stronger
assert lpi.get_num_rows() == 8
assert lpi.get_rhs()[row_index] == 0.9
# check verts()
verts = lpplot.get_verts(lpi)
assert len(verts) == 5
assert [0.0, 5.0] in verts
assert [0.9, 5.0] in verts
assert [0.0, 4.6] in verts
assert [0.9, 4.6] in verts
assert verts[0] == verts[-1]
def test_get_basis_matrix():
'tests lputil get_basis_matrix on harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
basis = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
mat = lputil.get_basis_matrix(lpi)
assert np.allclose(mat, basis)
def test_check_intersection():
'tests check_intersection on the harmonic oscillator example'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
# check if initially y >= 4.5 is possible (should be false)
direction = np.array([0, -1], dtype=float)
lc = LinearConstraint(direction, -4.5)
assert not lputil.check_intersection(lpi, lc)
# after basis matrix update
basis = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
# now check if y >= 4.5 is possible (should be true)
assert lputil.check_intersection(lpi, lc)
def test_chull_ha5():
'test convex hull aggregation of harmonic oscillator with 5 sets'
mode = HybridAutomaton().new_mode('mode_name')
steps = 5
step_size = math.pi/4
lpi_list = []
a_mat = np.array([[0, 1], [-1, 0]], dtype=float)
for step_num in range(steps):
box = [[-5, -4], [-0.5, 0.5]]
lpi = lputil.from_box(box, mode)
t = step_num * step_size
basis_mat = expm(a_mat * t)
lputil.set_basis_matrix(lpi, basis_mat)
lpi_list.append(lpi)
verts = []
for lpi in lpi_list:
verts += lpplot.get_verts(lpi)
xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'k-')
lpi = lputil.aggregate_chull(lpi_list, mode)
#xs, ys = zip(*lpplot.get_verts(lpi))
#plt.plot(xs, ys, 'r--')
#plt.show()
# test if it's really convex hull
assert lputil.is_point_in_lpi([0, 4.5], lpi)
for vert in verts:
assert lputil.is_point_in_lpi(vert, lpi)
def test_chull_lines():
'tests aggregation of two lines in 2d using convex hull'
mode = HybridAutomaton().new_mode('mode_name')
center = [-5, -1, 7]
generator = [0.5, 0.1, 1.0]
lpi = lputil.from_zonotope(center, [generator], mode)
t1 = math.pi / 3
a_mat = np.array([[-0.3, 1, 0], [-1, -0.3, 0], [0, 0.1, 1.1]], dtype=float)
bm = expm(a_mat * t1)
lputil.set_basis_matrix(lpi, bm)
lpi_list = [lpi.clone()]
all_verts = []
verts = lpplot.get_verts(lpi)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
t2 = t1 + 0.1
bm = expm(a_mat * t2)
lputil.set_basis_matrix(lpi, bm)
lpi_list.append(lpi.clone())
verts = lpplot.get_verts(lpi)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
chull_lpi = lputil.aggregate_chull(lpi_list, mode)
#xs, ys = zip(*lpplot.get_verts(chull_lpi))
#plt.plot(xs, ys, 'r--')
#plt.show()
for vert in all_verts:
assert lputil.is_point_in_lpi(vert, chull_lpi)
def apply_approx_chull(self):
'''
apply convex hull approximation model
'''
lpi_one_step = self.lpi.clone()
Timers.tic('get_bm')
bm, ie_mat = self.mode.time_elapse.get_basis_matrix(1)
Timers.toc('get_bm')
Timers.tic('set_bm')
lputil.set_basis_matrix(lpi_one_step, bm)
Timers.toc('set_bm')
if ie_mat is not None:
Timers.tic('input effects matrix')
lputil.add_input_effects_matrix(lpi_one_step, ie_mat, self.mode)
Timers.toc('input effects matrix')
lpi_list = [self.lpi, lpi_one_step]
self.lpi = lputil.aggregate_chull(lpi_list, self.mode)
def test_reset_less_dims():
'''tests a reset to a mode with less dimensions
project onto just the y variable multiplied by 0.5
'''
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
assert lpi.dims == 2
reset_csr = csr_matrix(np.array([[0, 0.5]], dtype=float))
mode_id = 1
transition_id = 13
lputil.add_reset_variables(lpi,
mode_id,
transition_id,
reset_csr=reset_csr)
assert lpi.dims == 1
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 0, 0], \
[0, 1, 0, -1, 0, 0], \
[-1, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0], \
[0, 0, 0, 0.5, -1, 0], \
[0, 0, 0, 0, 1, -1]], dtype=float)
expected_vec = np.array([0, 0, 5, -4, 0, 1, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx]
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1", "m1_i0_t13", "m1_c0"]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
assert lpi.basis_mat_pos == (7, 4)
assert lpi.dims == 1
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi,
xdim=0,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0)
assert len(verts) == 3
assert [0.5, 0] in verts
assert [0, 0] in verts
assert verts[0] == verts[-1]
# update the basis matrix
basis = np.array([[2]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
verts = lpplot.get_verts(lpi,
xdim=0,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0)
assert len(verts) == 3
assert [1.0, 0] in verts
assert [0, 0] in verts
assert verts[0] == verts[-1]
def test_chull_drivetrain():
'convex hull aggregation debugging from drivetrain system'
mode = HybridAutomaton().new_mode('mode_name')
center = [-0.0432, -11, 0, 30, 0, 30, 360, -0.0013, 30, -0.0013, 30, 0, 1]
generator = [0.0056, 4.67, 0, 10, 0, 10, 120, 0.0006, 10, 0.0006, 10, 0, 0]
lpi = lputil.from_zonotope(center, [generator], mode)
# neg_angle init dynamics
a_mat = np.array([ \
[0, 0, 0, 0, 0, 0, 0.0833333333333333, 0, -1, 0, 0, 0, 0], \
[13828.8888888889, -26.6666666666667, 60, 60, 0, 0, -5, -60, 0, 0, 0, 0, 116.666666666667], \
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -5], \
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], \
[0, 0, 0, 0, -714.285714285714, -0.04, 0, 0, 0, 714.285714285714, 0, 0, 0], \
[-2777.77777777778, 3.33333333333333, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -83.3333333333333], \
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], \
[100, 0, 0, 0, 0, 0, 0, -1000, -0.01, 1000, 0, 0, 3], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], \
[0, 0, 0, 0, 1000, 0, 0, 1000, 0, -2000, -0.01, 0, 0], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
], dtype=float)
# plot dimensions
xdim = 0
ydim = 1
step = 5.0E-2
t1 = 0
bm = expm(a_mat * t1)
lputil.set_basis_matrix(lpi, bm)
lpi_list = [lpi.clone()]
all_verts = []
verts = lpplot.get_verts(lpi, xdim=xdim, ydim=ydim)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
t2 = t1 + step
bm = expm(a_mat * t2)
lputil.set_basis_matrix(lpi, bm)
lpi_list.append(lpi.clone())
verts = lpplot.get_verts(lpi, xdim=xdim, ydim=ydim)
all_verts += verts
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'k-')
chull_lpi = lputil.aggregate_chull(lpi_list, mode)
plot_vecs = lpplot.make_plot_vecs(num_angles=256, offset=0.01)
verts = lpplot.get_verts(chull_lpi, xdim=xdim, ydim=ydim, plot_vecs=plot_vecs)
#xs, ys = zip(*verts)
#plt.plot(xs, ys, 'r--')
#plt.show()
for vert in all_verts:
assert lputil.is_point_in_lpi(vert, chull_lpi)
def test_reset_minkowski():
'''tests reset with a minkowski sum term and a new variable
pre reset we have x = [-5, -4], y = [0, 1]
post reset we have x = [-15, -14] (-10), y = [0, 1], t' = [0, 5]
reset_matrix is [[1, 0], [0, 1], [0, 0]]
minkowski_csr is [[1, 0], [0, 0], [0, 1]]
minkowski_constraints_csr is [[1, 0], [-1, 0], [0, 1], [0, -1]]
minkowski_constraints_rhs is [-10, 10, 5, 0]
'''
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
reset_csr = csr_matrix([[1, 0], [0, 1], [0, 0]], dtype=float)
mode_id = 1
transition_id = 13
minkowski_csr = csr_matrix([[1, 0], [0, 0], [0, 1]], dtype=float)
constraints_csr = csr_matrix([[1, 0], [-1, 0], [0, 1], [0, -1]],
dtype=float)
constraints_rhs = np.array([-10, 10, 5, 0], dtype=float)
lputil.add_reset_variables(lpi, mode_id, transition_id, reset_csr=reset_csr, minkowski_csr=minkowski_csr, \
minkowski_constraints_csr=constraints_csr, minkowski_constraints_rhs=constraints_rhs)
assert lpi.dims == 3
# basis matrix should be at 9, 6
assert lpi.basis_mat_pos == (9, 6)
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1", "reset0", "reset1", "m1_i0_t13", "m1_i1", "m1_i2", \
"m1_c0", "m1_c1", "m1_c2"]
assert lpi.get_names() == expected_names
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, xdim=0, ydim=1, plot_vecs=plot_vecs)
assert len(verts) == 5
assert [-15.0, 0.] in verts
assert [-15.0, 1.] in verts
assert [-14.0, 1.] in verts
assert [-14.0, 0.] in verts
assert verts[0] == verts[-1]
verts = lpplot.get_verts(lpi,
xdim=2,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0.0)
assert len(verts) == 3
assert [0, 0.] in verts
assert [5, 0.] in verts
assert verts[0] == verts[-1]
lputil.set_basis_matrix(lpi, 3 * np.identity(3))
verts = lpplot.get_verts(lpi,
xdim=2,
ydim=None,
plot_vecs=plot_vecs,
cur_time=0.0)
assert len(verts) == 3
assert [0, 0.] in verts
assert [15, 0.] in verts
assert verts[0] == verts[-1]
def apply_approx_lgg(self):
'''
apply lgg approximation model from equation (2) in Lemma 1 of:
"Reachability analysis of linear systems using support functions",
Le Guernic, C., Girard, A., Nonlinear Analysis: Hybrid Systems, 2010
'''
has_inputs = self.mode.b_csr is not None
# use infinity norm
a_norm = sp.sparse.linalg.norm(self.mode.a_csr, ord=np.inf)
lpi_one_step = self.lpi.clone()
Timers.tic('get_bm')
bm, _ = self.mode.time_elapse.get_basis_matrix(1)
Timers.toc('get_bm')
Timers.tic('set_bm')
lputil.set_basis_matrix(lpi_one_step, bm)
Timers.toc('set_bm')
mode = self.mode
tau = mode.time_elapse.step_size
r_x0 = lputil.compute_radius_inf(self.lpi)
if has_inputs:
v_set = lputil.from_input_constraints(mode.b_csr,
mode.u_constraints_csc,
mode.u_constraints_rhs, mode)
r_v = lputil.compute_radius_inf(v_set)
# minkowski sum with tau * V
# V is the input set, V = B U
lputil.scale_with_bm(v_set, tau)
msum = lputil.minkowski_sum([lpi_one_step, v_set], mode)
else:
r_v = 0
msum = lpi_one_step
tol = 1e-7
if a_norm < tol:
print(
f"Warning: norm of dynamics A matrix was small ({a_norm}), using alpha = 0 and "
+ "beta = 0 in LGG approximation model")
alpha = 0
else:
alpha = (math.exp(tau * a_norm) - 1 -
tau * a_norm) * (r_x0 + r_v / a_norm)
#print(f".ss alpha={alpha}")
if alpha != 0:
# bloat by alpha (minkowski sum)
lputil.bloat(msum, alpha)
self.lpi = lputil.aggregate_chull([self.lpi, msum], mode)
if a_norm > tol and has_inputs:
# precompute beta as well
self.lgg_beta = (math.exp(tau * a_norm) - 1 -
tau * a_norm) * (r_v / a_norm)
#print(f".ss beta={self.lgg_beta}")
assert self.lgg_beta > tol, f"lgg approx model beta was too close to zero: {self.lgg_beta}"
assert self.lgg_beta < 1e5, f"lgg approx model beta was too large (use a smaller step): {self.lgg_beta}"
self.lpi.set_minimize_direction([-1] * self.lpi.dims)
self.mode.time_elapse.use_lgg_approx()
def test_add_reset_variables():
'tests add_reset_variables'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
reset_csr = csr_matrix(2 * np.identity(2))
mode_id = 1
transition_id = 13
lputil.add_reset_variables(lpi,
mode_id,
transition_id,
reset_csr=reset_csr)
assert lpi.dims == 2
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 0, 0, 0, 0], \
[0, 1, 0, -1, 0, 0, 0, 0], \
[-1, 0, 0, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0, 0, 0], \
[0, 0, 2, 0, -1, 0, 0, 0], \
[0, 0, 0, 2, 0, -1, 0, 0], \
[0, 0, 0, 0, 1, 0, -1, 0], \
[0, 0, 0, 0, 0, 1, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 5, -4, 0, 1, 0, 0, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx, fx, fx]
expected_names = [
"m0_i0", "m0_i1", "m0_c0", "m0_c1", "m1_i0_t13", "m1_i1", "m1_c0",
"m1_c1"
]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
assert lpi.basis_mat_pos == (8, 4)
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert len(verts) == 5
assert [-10.0, 0.] in verts
assert [-10.0, 2.] in verts
assert [-8.0, 2.] in verts
assert [-8.0, 0.] in verts
assert verts[0] == verts[-1]
# update the basis matrix to rotate quarter circle
basis = np.array([[0, 1], [-1, 0]], dtype=float)
lputil.set_basis_matrix(lpi, basis)
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert len(verts) == 5
assert [0.0, 10.] in verts
assert [0.0, 8.] in verts
assert [2.0, 8.] in verts
assert [2.0, 10.] in verts
assert verts[0] == verts[-1]
def test_box_inputs():
'tests from_box with a simple input effects matrix'
# x' = Ax + Bu
# A = 0
# B = [[1, 0], [0, 2]]
# u1 and u2 are bounded between [1, 10]
# (init) step 0: [0, 1] x [0, 1]
# step 1: [1, 11] x [2, 21]
# step 2: [2, 21] x [4, 41]
mode = HybridAutomaton().new_mode('mode_name')
mode.set_dynamics(np.zeros((2, 2)))
mode.set_inputs([[1, 0], [0, 2]], [[1, 0], [-1, 0], [0, 1], [0, -1]],
[10, -1, 10, -1])
init_box = [[0, 1], [0, 1]]
lpi = lputil.from_box(init_box, mode)
assert lpi.basis_mat_pos == (0, 0)
assert lpi.dims == 2
assert lpi.cur_vars_offset == 2
assert lpi.input_effects_offsets == (
6, 4) # row 6, column 4 for total input effects offsets
# step 0
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 1, 0], \
[0, 1, 0, -1, 0, 1], \
[-1, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0], \
[0, 0, 0, 0, -1, 0], \
[0, 0, 0, 0, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 0, 1, 0, 1, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx]
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1", "m0_ti0", "m0_ti1"]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, init_box)
# do step 1
mode.init_time_elapse(1.0)
basis_mat, input_mat = mode.time_elapse.get_basis_matrix(1)
lputil.set_basis_matrix(lpi, basis_mat)
lputil.add_input_effects_matrix(lpi, input_mat, mode)
mat = lpi.get_full_constraints()
types = lpi.get_types()
rhs = lpi.get_rhs()
names = lpi.get_names()
expected_mat = np.array([\
[1, 0, -1, 0, 1, 0, 0, 0], \
[0, 1, 0, -1, 0, 1, 0, 0], \
[-1, 0, 0, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0, 0, 0], \
[0, 0, 0, 0, -1, 0, 1, 0], \
[0, 0, 0, 0, 0, -1, 0, 2], \
[0, 0, 0, 0, 0, 0, 1, 0], \
[0, 0, 0, 0, 0, 0, -1, 0], \
[0, 0, 0, 0, 0, 0, 0, 1], \
[0, 0, 0, 0, 0, 0, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 0, 1, 0, 1, 0, 0, 10, -1, 10, -1],
dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up, up, fx, fx, up, up, up, up]
expected_names = [
"m0_i0", "m0_i1", "m0_c0", "m0_c1", "m0_ti0", "m0_ti1", "m0_I0",
"m0_I1"
]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(1, 11), (2, 21)])
# do step 2
basis_mat, input_mat = mode.time_elapse.get_basis_matrix(2)
lputil.set_basis_matrix(lpi, basis_mat)
lputil.add_input_effects_matrix(lpi, input_mat, mode)
verts = lpplot.get_verts(lpi)
assert_verts_is_box(verts, [(2, 21), (4, 41)])
