def compute_pre(pwa_system: PiecewiseAffineSys, X: StateCell, input_range: Box,
target: Box):
winning_size = np.zeros(target.center.shape)
start_time = time.time()
X_new = deepcopy(X)
cell_list = [X_new]
while cell_list:
# if time.time() - start_time > 10:
# break
x = cell_list.pop(0)
if x.fully_solved():
# print("hoi")
continue
if np.all(x.as_box().get_bounding_box_size() <
target.get_bounding_box_size() / 16):
break
# print(x.as_box().get_bounding_box_size())
assert isinstance(x, StateCell)
# if np.all(x.as_box().get_bounding_box_size() < winning_size / 4):
# print("not worth it")
# break
parent_cell = pwa_system.get_parent_cell(x)
affine_dynamics = parent_cell.get_multistep_dynamics()
reachable_set = affine_dynamics.compute_reachable_set(
x.as_box(), input_range)
if not reachable_set.get_bounding_box().intersects(target):
# print('boo')
continue
cl_dynamics, alpha, feedback_rule = parent_cell.get_closed_loop_dynamics(
input_range, target)
x.multi_step_dynamics = deepcopy(affine_dynamics)
x.closed_loop_dynamics = deepcopy(cl_dynamics)
assert isinstance(alpha, float)
if alpha > 0.0:
x.feedback_control = feedback_rule
tolerance = (target.get_bounding_box_size() -
cl_dynamics.compute_reachable_set(
x.as_box()).get_bounding_box_size()) / 2
success, ctrl = steer(affine_system=affine_dynamics,
start_state=x.as_box().center,
input_range=(1 - alpha) * input_range,
target_state=target.center,
tolerance=tolerance)
# else:
# print("no feedback")
# success = False
if success:
winning_size = np.maximum(winning_size,
x.as_box().get_bounding_box_size())
x.is_winning = True
x.feedfwd_control = ctrl
else:
if x.has_child():
cell_list = cell_list + x.children
else:
x.split_cell()
cell_list = cell_list + x.children
return X_new
def __call__(self, x, u):
if isinstance(x, np.ndarray):
x = x.reshape(x.size, 1)
x = Box(np.concatenate((x, x), axis=1))
if isinstance(u, np.ndarray):
u = u.reshape(u.size, 1)
u = Box(np.concatenate((u, u), axis=1))
assert isinstance(x, Zonotope)
assert isinstance(u, Zonotope)
self.compute_reachable_set(x, u)
def get_ru(self, input: [Box, np.ndarray]) -> Zonotope:
if isinstance(input, Box):
return self.B * input
elif isinstance(input, np.ndarray):
output = self.B @ input
output = output.reshape((len(output), 1))
output_as_range = np.concatenate((output, output), 1)
output_box = Box(output_as_range)
return output_box
elif isinstance(input, float):
output = self.B * input
output = output.reshape((len(output), 1))
output_as_range = np.concatenate((output, output), 1)
output_box = Box(output_as_range)
return output_box
def get_affine_dynamics(F,
state_region: Box,
input_region: Box,
n_sample=1000):
samples_x = state_region.sample(n_sample)
samples_u = input_region.sample(n_sample)
samples_in = np.concatenate((samples_x, samples_u), axis=0)
samples_x_plus = F(samples_x, samples_u)
affine_func = fit_affine_function(input_data=samples_in,
output_data=samples_x_plus)
affine_system = AffineSys()
affine_system.init_from_affine_func(affine_func=affine_func,
state_index_list=list(
range(0, state_region.ndim)))
return affine_system
def __init__(self, input_box: Box, state_box: Box, refinement_list: list):
"""
:param input_box: allowed inputs
:param state_box: The state space where the hybridization is consturcteed
:param refinement_list: List of the state dimensions that can be refined during hybridization
"""
self.state_cell = StateCell(state_box.get_range())
self.input_box = input_box
self.state_cell.refinement_list = refinement_list
self.size = 1
def get_parent_cell(self, x: Box) -> StateCell:
if isinstance(x, np.ndarray):
x = Box(np.array([[x[0], x[0]], [x[1], x[1]]]))
elif isinstance(x, StateCell):
x = x.as_box()
assert isinstance(x, Box)
if not self.state_cell.as_box().contains(x):
print("State out of range")
return None
smallest_cell = self.state_cell
while smallest_cell.has_child():
children = smallest_cell.children
found_valid = False
for ch in children:
if ch.as_box().contains(x):
smallest_cell = ch
found_valid = True
break
if not found_valid:
break
return smallest_cell
def __call__(self, state_box: ztp.Box, input):
input_usage = np.abs(
self.feedback_rule) @ state_box.get_bounding_box_size()
input_usage = np.max(input_usage /
self.input_box.get_bounding_box_size())
if input_usage > self.budget:
temp_fdb = self.budget / input_usage * self.feedback_rule
else:
temp_fdb = self.feedback_rule
reachset = self.system.compute_reachable_set_cl(
temp_fdb, state_box, input)
return reachset
def compute_hybridization(self, F, precision: Box, n_time_steps: int,
input_box: Box):
cell_list = [self.state_cell]
while cell_list:
cell = cell_list.pop(0)
cell.dynamics = get_affine_dynamics(F, cell.as_box(), input_box,
2000)
# cell.multi_step_dynamics = get_multistep_system(cell.dynamics, n_time_steps)
cell.compute_multistep_affine(F, n_time_steps, input_box)
if not precision.contains(cell.multi_step_dynamics.W):
cell.split_cell()
cell_list = cell_list + cell.children
self.size += len(cell.children)
def get_attraction(F: AffineSys, target: Box, input_region: Box, n_steps: int):
state_region = deepcopy(target)
F_affine = get_affine_dynamics(F, state_region, input_region)
F_affine_multi = get_multistep_system(F_affine, n_steps)
while np.all(F_affine_multi.W.get_bounding_box_size() <
target.get_bounding_box_size()):
F_affine_multi_old = deepcopy(F_affine_multi)
state_region_old = deepcopy(state_region)
state_region = 1.1 * state_region
F_affine = get_affine_dynamics(F, state_region, input_region)
F_affine_multi = get_multistep_system(F_affine, n_steps)
F_affine_multi = deepcopy(F_affine_multi_old)
state_region = deepcopy(state_region_old)
success, feedback_rule, alpha, closed_loop_system = synthesize_controller(
F_affine_multi, 1.01 * state_region, input_region, state_region)
return state_region, feedback_rule
def intersects(self, box: ztp.Box):
state_range = box.get_range()
state_patch = self.winset.get_patch(state_range)
if state_patch is not None and np.any(state_patch > 0):
return True
return False
def is_included(self, box: ztp.Box):
state_range = box.get_range()
state_patch = self.winset.get_patch(state_range)
if state_patch is not None and np.all(state_patch > 0):
return True
return False
def compute_pre2(F, n_time_steps, X: StateCell, input_range: Box, target: Box):
winning_size = np.zeros(target.center.shape)
input_range_multi = input_range.get_range()
input_range_multi = Box(np.tile(input_range_multi, (n_time_steps, 1)))
X_new = deepcopy(X)
cell_list = [X_new]
while cell_list:
cell = cell_list.pop(0)
if cell.fully_solved():
continue
if np.all(cell.as_box().get_bounding_box_size() <
target.get_bounding_box_size() / 16):
continue
if np.all(cell.as_box().get_bounding_box_size() < winning_size / 2):
print("not worth it")
break
cell.compute_multistep_affine(F, n_time_steps, input_range)
assert isinstance(cell.multi_step_dynamics, AffineSys)
ru = cell.multi_step_dynamics.get_ru(input_range_multi)
ru_inv = copy(ru)
ru_inv.center = -ru_inv.center
pre_target_over = target.minkowski_sum(
cell.multi_step_dynamics.W).minkowski_sum(ru_inv)
if np.all(target.get_bounding_box_size() >=
cell.multi_step_dynamics.W.get_bounding_box_size()):
target_under = target.get_bounding_box()
target_under.generators = target.generators - cell.multi_step_dynamics.W.get_bounding_box(
).generators
target_under.center = target_under.center - cell.multi_step_dynamics.W.center
pre_target_under = target_under.minkowski_sum(ru_inv)
else:
pre_target_under = None
sub_list = [cell]
while sub_list:
sub_cell = sub_list.pop(0)
if np.all(
sub_cell.as_box().get_bounding_box_size() < winning_size /
2):
print("not worth it")
break
if np.all(sub_cell.as_box().get_bounding_box_size() <
target.get_bounding_box_size() / 16):
continue
assert isinstance(sub_cell.multi_step_dynamics, AffineSys)
rx = sub_cell.multi_step_dynamics.get_rx(sub_cell.as_box())
if pre_target_over.intersects(rx):
if pre_target_over.contains(rx):
if pre_target_under and pre_target_under.contains(rx):
winning_size = np.maximum(
winning_size,
sub_cell.as_box().get_bounding_box_size())
sub_cell.is_winning = True
else:
sub_cell.split_cell()
cell_list = cell_list + sub_cell.children
else:
sub_cell.split_cell()
sub_list = sub_list + sub_cell.children
else:
continue
return X_new
def as_box(self):
b = Box(self.range)
return b
umax = -umin
x_0 = np.array([1, 1])
run_time = 2
t_range = np.arange(0, run_time, sample_time)
x = deepcopy(x_0)
x_hist = np.zeros((2, len(t_range)))
for i, t in enumerate(t_range):
x_hist[:, i] = x.reshape((2, ))
x = F(x, np.array([0]))
# finding invariant set around the upright position
target_point = np.array([0, 0]).reshape((2, 1))
tolerance = np.array([0.2, 0.2]).reshape((2, 1))
target_region = np.concatenate(
(target_point - tolerance, target_point + tolerance), axis=1)
target_set = Box(target_region)
input_set = Box(np.array([umin, umax]).reshape(1, 2))
affine_system = get_affine_dynamics(F, target_set, input_set)
n_time_steps = 5
multistep_system = get_multistep_system(affine_system=affine_system,
n_time_steps=n_time_steps)
print(multistep_system.W.get_bounding_box_size())
feed_back_law = synthesize_controller(multistep_system, 1.5 * target_set,
input_set, 1 * target_set)
# running the closed loop system
x = deepcopy(x_0)
x_hist = np.zeros((2, len(t_range)))
feed_back_plan = np.zeros((2, 0))
for i, t in enumerate(t_range):