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 init_plot_vecs(self):
'initialize plot_vecs'
if isinstance(self.settings.xdim_dir, list):
assert isinstance(self.settings.ydim_dir, list)
assert len(self.settings.xdim_dir) == len(self.settings.ydim_dir)
else:
self.settings.xdim_dir = [self.settings.xdim_dir]
self.settings.ydim_dir = [self.settings.ydim_dir]
for xdim_dir, ydim_dir in zip(self.settings.xdim_dir, self.settings.ydim_dir):
assert not (xdim_dir is None and ydim_dir is None)
plot_vecs = []
if xdim_dir is None:
plot_vecs.append(np.array([0, 1.], dtype=float))
plot_vecs.append(np.array([0, -1.], dtype=float))
elif self.settings.ydim_dir is None:
plot_vecs.append(np.array([1., 0], dtype=float))
plot_vecs.append(np.array([-1., 0], dtype=float))
else:
assert self.settings.num_angles >= 3, "needed at least 3 directions in plot_settings.num_angles"
plot_vecs = lpplot.make_plot_vecs(self.settings.num_angles)
self.plot_vec_list.append(plot_vecs)
self.num_subplots = len(self.plot_vec_list)
def test_verts():
'tests verts'
lpi = lputil.from_box([[-5, -4], [0, 1]],
HybridAutomaton().new_mode('mode_name'))
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert_verts_is_box(verts, [(-5, -4), (0, 1)])
def test_init_triangle():
'tests initialization from a non-box initial set of states'
# x + y < 1, x > 0, y > 0
constraints_mat = [[1, 1], [-1, 0], [0, -1]]
constraints_rhs = [1, 0, 0]
lpi = lputil.from_constraints(constraints_mat, constraints_rhs,
HybridAutomaton().new_mode('mode_name'))
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, 1, 0, -1], \
[1, 1, 0, 0], \
[-1, 0, 0, 0], \
[0, -1, 0, 0]], dtype=float)
expected_vec = np.array([0, 0, 1, 0, 0], dtype=float)
fx = glpk.GLP_FX
up = glpk.GLP_UP
expected_types = [fx, fx, up, up, up]
expected_names = ["m0_i0", "m0_i1", "m0_c0", "m0_c1"]
assert np.allclose(rhs, expected_vec)
assert types == expected_types
assert np.allclose(mat.toarray(), expected_mat)
assert names == expected_names
# check verts
plot_vecs = lpplot.make_plot_vecs(4, offset=(math.pi / 4.0))
verts = lpplot.get_verts(lpi, plot_vecs=plot_vecs)
assert len(verts) == 4
assert [0., 1.] in verts
assert [0., 0.] in verts
assert [1., 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 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_add_reset_inputs():
'tests add_reset_variables'
mode = HybridAutomaton().new_mode('mode_name')
lpi = lputil.from_box([[-5, -4], [0, 1]], mode)
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,
successor_has_inputs=True)
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, 0], \
[0, 1, 0, -1, 0, 0, 0, 0, 0, 0], \
[-1, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, -1, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0], \
[0, 0, 2, 0, -1, 0, 0, 0, 0, 0], \
[0, 0, 0, 2, 0, -1, 0, 0, 0, 0], \
[0, 0, 0, 0, 1, 0, -1, 0, 1, 0], \
[0, 0, 0, 0, 0, 1, 0, -1, 0, 1], \
[0, 0, 0, 0, 0, 0, 0, 0, -1, 0], \
[0, 0, 0, 0, 0, 0, 0, 0, 0, -1]], dtype=float)
expected_vec = np.array([0, 0, 5, -4, 0, 1, 0, 0, 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, fx, fx]
expected_names = [
"m0_i0", "m0_i1", "m0_c0", "m0_c1", "m1_i0_t13", "m1_i1", "m1_c0",
"m1_c1", "m1_ti0", "m1_ti1"
]
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)
assert lpi.input_effects_offsets == (10, 8)
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]