import zonotope_lib as ztp
import pwa_lib as pwa
import matplotlib.animation as animation
f = PendulumCartContinuous()
sample_time = 0.01
F = SampleAndHold(continuous_function=f, sample_time=sample_time)
run_time = 0.12
state_region = ztp.Box(np.array([[-np.pi / 10, np.pi / 10], [-100, 100]]))
input_region = ztp.Box(np.array([[-1, 1]]))
F_linear = pwa.get_affine_dynamics(F=F,
state_region=state_region,
input_region=input_region,
n_sample=2000)
n_time_steps = 2
F_linear_multistep = pwa.get_multistep_system(affine_system=F_linear,
n_time_steps=n_time_steps)
min_cell_size = F_linear_multistep.W.get_bounding_box()
# feed_back_law, alpha = pwa.synthesize_controller(F_linear_multistep, 0.2*min_cell_size, input_region,
# 1.00*min_cell_size)
# ztp.plot_zonotope(ztp.Zonotope(10*F_linear_multistep.B, np.array([[0], [0]])).get_bounding_box(), color='b')
# ztp.plot_zonotope(ztp.Zonotope(F_linear_multistep.A, np.array([[0], [0]])), color='b')
input_reach = ztp.Zonotope(np.array([[], []]), np.array([[0.0], [0.0]]))
n_inputs = input_region.ndim
for i in range(n_time_steps):
input_reach = input_reach + F_linear_multistep.B[:, n_inputs * i:n_inputs *
(i + 1)] * input_region
ztp.plot_zonotope(input_reach)
ztp.plot_zonotope(state_regions[-1], fill=False)
f = PendulumCartContinuous()
sample_time = 0.01
F = SampleAndHold(continuous_function=f, sample_time=sample_time)
F_linear = []
F_linear_multistep = []
feed_back_law = []
for id, reg in enumerate(state_regions):
F_linear.append(
pwa.get_affine_dynamics(F=F,
state_region=reg,
input_region=input_region))
F_linear_multistep.append(
pwa.get_multistep_system(affine_system=F_linear[-1],
n_time_steps=n_time_steps))
feed_back_law.append(
pwa.synthesize_controller(F_linear_multistep[-1], unit_cell,
input_region, 0.99 * unit_cell))
cells = []
winning_cells = []
for i in np.arange(x_start, x_end, unit_cell_size[0][1] * 2):
for j in np.arange(y_start, x_end, unit_cell_size[0][1] * 2):
center = np.array([[i], [j]])
new_cell = ztp.Box(center + unit_cell_size)
cells.append(copy(new_cell))
if np.all(np.abs(center) < 0.01) and np.all(np.abs(center) > -0.01):
winning_cells.append(copy(new_cell))
solved_winning_cells = []