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 test_chull():
'tests aggregation of a cirle of sets using convex hull'
mode = HybridAutomaton().new_mode('mode_name')
r = 1.0
eps = 0.05
num_sets = 16
lpi_list = []
for theta in np.linspace(0, 2*math.pi, num_sets, endpoint=False):
y = r * math.sin(theta)
x = r * math.cos(theta)
mat = [[1, 0], [-1, 0], [0, 1], [0, -1]]
rhs = [x + eps, -(x - eps), y + eps, -(y - eps)]
lpi = lputil.from_constraints(mat, rhs, mode)
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--')
#for vert in verts:
# assert lputil.is_point_in_lpi(vert, lpi)
#plt.show()
# test if it's really convex hull
for theta in np.linspace(0, 2*math.pi, num_sets):
y = (r + 2*eps) * math.sin(theta)
x = (r + 2*eps) * math.cos(theta)
assert not lputil.is_point_in_lpi([x, y], lpi)
y = (r - 2*eps) * math.sin(theta)
x = (r - 2*eps) * math.cos(theta)
assert lputil.is_point_in_lpi([x, y], lpi)
def aggregate_chull(agg_list, op_list, print_func):
'''
perform template-based aggregation on the passed-in list of states
Currently, this can either use box template directions or arnoldi (+box) template directions
'''
min_step = min([state.cur_steps_since_start[0] for state in agg_list])
max_step = max([state.cur_steps_since_start[1] for state in agg_list])
step_interval = [min_step, max_step]
print_func("Convex hull aggregation time step interval: {}".format(step_interval))
postmode = agg_list[0].mode
lpi_list = [state.lpi for state in agg_list]
new_lpi = lputil.aggregate_chull(lpi_list, postmode)
return StateSet(new_lpi, agg_list[0].mode, step_interval, op_list, is_concrete=False)
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 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 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_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 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()