from dynamics_library import SampleAndHold, PendulumCartContinuous
from copy import copy
import matplotlib
import matplotlib.pyplot as plt
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):
sample_time=sample_time,
discretization_step=0.01)
input_min = -1
input_max = -input_min
input_box = ztp.Box(np.array([[input_min, input_max]]))
theta_min = -4
theta_max = 4
theta_dot_min = -4
theta_dot_max = 4
n_steps = 2
target = ztp.Box(np.array([[-.2, .2], [-.2, .2]]))
input_box_multistep = ztp.Box(
np.tile(np.array([[input_min, input_max]]), (n_steps, 1)))
state_box = ztp.Box(
np.array([[theta_min, theta_max], [theta_dot_min, theta_dot_max]]))
affine_dynamics = pwa.get_affine_dynamics(F, state_box, input_box)
reachset_func = lambda x, u: affine_dynamics.compute_reachable_set(x, u)
winning_check = WinSetCheck(state_box.get_range(), np.array([[0.01], [0.01]]))
winning_check.winset.set_patch(target.get_range(), 1)
# fig, ax = plt.subplots()
# im = ax.imshow(winning_check.winset.map)
# plt.show()
cells = pwa.StateCell(state_box.get_range())
input_list = list(np.arange(input_min, input_max, 0.02))
check_include = winning_check.is_included
check_intersect = winning_check.intersects
iterations = 5
# cells = H.state_cell.get_bare_copy()
state_regions.append(ztp.Box(copy(region_limits)))
state_regions_feedback.append(0)
state_regions_alpha.append(0)
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))