def find_closest_state(self, query_point, save_true_dynamics_path=False):
'''
Find the closest state from the query point to a given polytope
:param query_point:
:return: Tuple (closest_point, closest_point_is_self.state)
'''
distance = np.inf
closest_point = None
p_used = None
try:
#use AABB to upper bound distance
min_dmax = np.inf
for i, aabb in enumerate(self.aabb_list):
dmax = point_to_box_dmax(query_point, aabb)
if dmax < min_dmax:
min_dmax = dmax
for i, p in enumerate(self.polytope_list):
#ignore polytopes that are impossible
if point_to_box_distance(query_point, self.aabb_list[i]) > min_dmax:
continue
d, proj = distance_point_polytope(p, query_point, ball='l2')
if d<distance:
distance = d
closest_point = proj
p_used = p
assert closest_point is not None
except TypeError:
closest_point = distance_point_polytope(self.polytope_list, query_point, ball='l2')[1]
p_used = self.polytope_list
if np.linalg.norm(closest_point-self.parent_state)<self.epsilon:
if save_true_dynamics_path:
return np.ndarray.flatten(closest_point), True, np.asarray([])
return np.ndarray.flatten(closest_point), True, np.asarray([self.parent_state, np.ndarray.flatten(closest_point)])
return np.ndarray.flatten(closest_point), False, np.asarray([self.parent_state, np.ndarray.flatten(closest_point)])
def contains(self, goal_state, return_closest_state = True):
# print(distance_point(self.polytope, goal_state)[0])
# print(self.polytope)
try:
#multimodal
distance = np.inf
closest_state = None
for i, polytope in enumerate(self.polytope_list):
# if point_to_box_distance(goal_state, self.aabb_list[i])>0:
# continue
current_distance, current_closest_state = distance_point_polytope(polytope, goal_state, ball='l2')
if current_distance < self.epsilon:
if return_closest_state:
return True, goal_state
else:
return True
else:
if current_distance < distance:
distance = current_distance
closest_state = current_closest_state
return False, closest_state
except TypeError:
distance, closest_state = distance_point_polytope(self.polytope_list, goal_state, ball='l2')
if distance < self.epsilon:
if return_closest_state:
return True, closest_state
else:
return True
if return_closest_state:
return False, closest_state
return False
def contains_goal_function(reachable_set, goal_state):
distance = np.inf
projection = None
# reachable set is a singular point
# if reahc
#
# else:
for i, p in enumerate(reachable_set.polytope_list):
if (p.T == 0.).all():
# reachable set is a line
# check if the goal is on the line
vec_to_goal = goal_state - reachable_set.parent_state
vec_to_reachable_set = np.ndarray.flatten(
p.t) - reachable_set.parent_state
# normalize
vec_to_reachable_set_norm = np.linalg.norm(
vec_to_reachable_set)
vec_dot = np.dot(vec_to_reachable_set,
vec_to_goal) / vec_to_reachable_set_norm
crosstrack_vec = vec_to_goal - vec_dot * vec_to_reachable_set / vec_to_reachable_set_norm
if np.linalg.norm(
crosstrack_vec
) < distance and 0. <= vec_dot <= vec_to_reachable_set_norm:
distance = np.linalg.norm(crosstrack_vec)
else:
d, proj = distance_point_polytope(p, goal_state)
if d < distance:
projection = proj
distance = d
if distance < goal_tolerance:
return True
return False
def set_polytope_pair_distance(arguments):
key_points, key_point_to_polytope_map, polytope_index, key_point_index = arguments
key_point = key_points[key_point_index]
key_point_string = str(key_point)
polytope = key_point_to_polytope_map[key_point_string]['polytopes'][
polytope_index]
return distance_point_polytope(to_AH_polytope(polytope),
key_point,
ball='l2')[0]
def find_closest_polytope(self,
query_point,
return_intermediate_info=False):
#find the closest centroid
d, i = self.key_point_tree.query(query_point)
closest_key_point = self.key_point_tree.data[i]
# print('closest key point', closest_key_point)
closest_key_point_polytope = self.key_point_to_polytope_map[str(
closest_key_point)]['polytopes'][0]
# print('closest polytope centroid' + str(closest_key_point_polytope.x))
dist_query_centroid_polytope = distance_point_polytope(
closest_key_point_polytope, query_point, ball='l2')[0]
dist_query_key_point = np.linalg.norm(query_point - closest_key_point)
# print(dist_query_key_point, dist_query_centroid_polytope)
cutoff_index = np.searchsorted(
self.key_point_to_polytope_map[str(closest_key_point)].distances,
dist_query_key_point + dist_query_centroid_polytope)
# print(cutoff_index)
# print(self.key_point_to_polytope_map[str(closest_key_point)]['distances'][0:cutoff_index])
# print(self.key_point_to_polytope_map[str(closest_key_point)]['distances'][cutoff_index:])
# print('dqc',dist_query_key_point)
# print(self.centroid_to_polytope_map[str(closest_key_point)].distances)
closest_polytope_candidates = self.key_point_to_polytope_map[str(
closest_key_point)].polytopes[0:cutoff_index]
# print(closest_polytope_candidates)
best_polytope = None
best_distance = np.inf
for polytope in closest_polytope_candidates:
if best_distance < 1e-9:
break
dist = distance_point_polytope(polytope, query_point, ball='l2')[0]
if best_distance > dist:
best_distance = dist
best_polytope = polytope
# print('best distance', best_distance)
if return_intermediate_info:
return best_polytope, best_distance, closest_polytope_candidates
return best_polytope
def contains_goal_function(reachable_set, goal_state):
distance = np.inf
if np.linalg.norm(reachable_set.parent_state - goal_state) < 2:
distance, projection = distance_point_polytope(
reachable_set.polytope_list, goal_state)
elif np.linalg.norm(reachable_set.parent_state - goal_state_2) < 2:
distance, projection = distance_point_polytope(
reachable_set.polytope_list, goal_state_2)
else:
return False, None
if distance > reachable_set_epsilon:
return False, None
#enumerate inputs
potential_inputs = np.linspace(pendulum_system.input_limits[0, 0],
pendulum_system.input_limits[1, 0],
input_samples)
for u_i in potential_inputs:
state_list = []
state = reachable_set.parent_state
for step in range(int(step_size / nonlinear_dynamic_step_size)):
state = pendulum_system.forward_step(
u=np.atleast_1d(u_i),
linearlize=False,
modify_system=False,
step_size=nonlinear_dynamic_step_size,
return_as_env=False,
starting_state=state)
state_list.append(state)
if np.linalg.norm(goal_state - state) < goal_tolerance:
print('Goal error is %d' %
np.linalg.norm(goal_state - state))
return True, np.asarray(state_list)
if np.linalg.norm(goal_state_2 - state) < goal_tolerance:
print('Goal error is %d' %
np.linalg.norm(goal_state_2 - state))
return True, np.asarray(state_list)
return False, None
def contains_goal_function(reachable_set, goal_state):
global best_distance
distance = np.inf
projection = None
# reachable set is a singular point
for i, p in enumerate(reachable_set.polytope_list):
# if (p.T==0.).all():
# # reachable set is a line
# # check if the goal is on the line
# vec_to_goal = goal_state - reachable_set.parent_state
# vec_to_reachable_set = np.ndarray.flatten(p.t)-reachable_set.parent_state
# # normalize
# vec_to_reachable_set_norm = np.linalg.norm(vec_to_reachable_set)
# vec_dot = np.dot(vec_to_reachable_set, vec_to_goal)/vec_to_reachable_set_norm
# crosstrack_vec = vec_to_goal-vec_dot*vec_to_reachable_set/vec_to_reachable_set_norm
# if np.linalg.norm(crosstrack_vec)<distance and 0.<=vec_dot<=vec_to_reachable_set_norm:
# distance = np.linalg.norm(crosstrack_vec)
# else:
d, proj = distance_point_polytope(p, goal_state, ball='l2')
if d < distance:
projection = proj
distance = d
# if distance<0.5:
# # enumerate inputs
# potential_inputs = np.linspace(hopper_system.input_limits[0, 0], hopper_system.input_limits[1, 0],
# input_samples)
# for u_i in potential_inputs:
# state_list = []
# state = reachable_set.parent_state
# for step in range(int(step_size / nonlinear_dynamic_step_size)):
# state = hopper_system.forward_step(u=np.atleast_1d(u_i), linearlize=False, modify_system=False,
# step_size=nonlinear_dynamic_step_size, return_as_env=False,
# starting_state=state)
# state_list.append(state)
# distance = np.linalg.norm(goal_state-state)
best_distance = min(distance, best_distance)
if distance < goal_tolerance:
print('Goal error is %f' % distance)
return True, [reachable_set.parent_state, goal_state]
return False, None
def find_closest_polytopes(self,
original_query_point,
return_intermediate_info=False,
return_state_projection=False,
may_return_multiple=False):
#find closest centroid
# try:
# query_point.shape[1]
# #FIXME: Choose between d*1 (2D) or d (1D) representation of query_point
# except:
# # raise ValueError('Query point should be d*1 numpy array')
# query_point=query_point.reshape((-1,1))
# Construct centroid box
scaled_query_point = np.multiply(self.distance_scaling_array,
original_query_point.flatten())
_x, ind = self.scaled_key_point_tree.query(
np.ndarray.flatten(scaled_query_point))
scaled_closest_centroid = self.scaled_key_point_tree.data[ind]
#Use dist(polytope, query) as upper bound
evaluated_zonotopes = []
centroid_zonotopes = self.key_point_to_zonotope_map[str(
np.divide(scaled_closest_centroid, self.distance_scaling_array))]
polytope_state_projection = {}
dist_to_query = {}
# inf_dist_to_query = {}
assert (len(centroid_zonotopes) == 1)
evaluated_zonotopes.extend(centroid_zonotopes)
zd, state = distance_point_polytope(
centroid_zonotopes[0],
original_query_point,
ball='l2',
distance_scaling_array=self.distance_scaling_array)
# zd = distance_point_polytope(cz, query_point, ball='l2')[0]
best_scaled_distance = zd
# best_inf_distance=zd
best_polytope = {centroid_zonotopes[0]}
dist_to_query[centroid_zonotopes[0]] = best_scaled_distance
polytope_state_projection[centroid_zonotopes[0]] = state
# inf_dist_to_query[cz] = best_inf_distance
# scale for numerical reasons
scaled_aabb_offsets = np.abs(state.flatten() -
original_query_point.flatten()) * 1.001
u = original_query_point.flatten() - scaled_aabb_offsets
v = original_query_point.flatten() + scaled_aabb_offsets
heuristic_box_lu = np.concatenate([u, v])
# scale the query box
scaled_heuristic_box_lu = np.multiply(self.repeated_scaling_matrix,
heuristic_box_lu)
#create query box
#find candidate box nodes
candidate_ids = list(self.idx.intersection(scaled_heuristic_box_lu))
# print('Evaluating %d zonotopes') %len(candidate_boxes)
#map back to zonotopes
if len(candidate_ids) == 0:
# This should never happen
raise ValueError('No closest zonotope found!')
# When a heuristic less than centroid distance is used,
# a candidate box does not necessarily exist. In this case,
# use the zonotope from which the heuristic is generated.
# '''
#
# evaluated_zonotopes = pivot_polytope
# closest_distance = pivot_distance
# return evaluated_zonotopes, candidate_boxes, query_box
else:
# for cb in candidate_boxes:
# print(cb)
# evaluated_zonotopes.append(cb.polytope)
#find the closest zonotope with randomized approach]
while (len(candidate_ids) >= 1):
if best_scaled_distance < 1e-9:
# point is contained by polytope, break
break
sample = np.random.randint(len(candidate_ids))
#solve linear program for the sampled polytope
pivot_polytope = self.index_to_polytope_map[
candidate_ids[sample]]
if pivot_polytope in best_polytope:
#get rid of this polytope
candidate_ids[sample], candidate_ids[-1] = candidate_ids[
-1], candidate_ids[sample]
candidate_ids = candidate_ids[0:-1]
continue
if pivot_polytope not in dist_to_query:
pivot_distance, state = distance_point_polytope(
pivot_polytope,
original_query_point,
ball="l2",
distance_scaling_array=self.distance_scaling_array)
# inf_pivot_distance = distance_point_polytope(pivot_polytope, query_point)[0]
dist_to_query[pivot_polytope] = pivot_distance
polytope_state_projection[pivot_polytope] = state
# inf_dist_to_query[pivot_polytope] = inf_dist_to_query
if return_intermediate_info:
evaluated_zonotopes.append(pivot_polytope)
else:
pivot_distance = dist_to_query[pivot_polytope]
# inf_pivot_distance = inf_dist_to_query[pivot_polytope]
if pivot_distance > best_scaled_distance: #fixme: >= or >?
#get rid of this polytope
candidate_ids[sample], candidate_ids[-1] = candidate_ids[
-1], candidate_ids[sample]
candidate_ids = candidate_ids[0:-1]
elif np.allclose(pivot_distance, best_scaled_distance):
best_polytope.add(pivot_polytope)
#get rid of this polytope
candidate_ids[sample], candidate_ids[-1] = candidate_ids[
-1], candidate_ids[sample]
candidate_ids = candidate_ids[0:-1]
else:
#reconstruct AABB
# create query box
# scale for numerical reasons
scaled_aabb_offsets = np.abs(
state.flatten() -
original_query_point.flatten()) * 1.001
u = original_query_point.flatten() - scaled_aabb_offsets
v = original_query_point.flatten() + scaled_aabb_offsets
heuristic_box_lu = np.concatenate([u, v])
# scale the query box
scaled_heuristic_box_lu = np.multiply(
self.repeated_scaling_matrix, heuristic_box_lu)
# find new candidates
candidate_ids = list(
self.idx.intersection(scaled_heuristic_box_lu))
best_scaled_distance = pivot_distance
# best_inf_distance = inf_pivot_distance
best_polytope = {pivot_polytope}
if return_intermediate_info:
return np.atleast_1d(
list(best_polytope)[0]
), best_scaled_distance, evaluated_zonotopes, heuristic_box_lu
if return_state_projection:
if not may_return_multiple:
return np.atleast_1d(
list(best_polytope)
[0]), best_scaled_distance, polytope_state_projection[
list(best_polytope)[0]]
else:
return np.asarray(
list(best_polytope)), best_scaled_distance, np.asarray(
[
polytope_state_projection[bp].flatten()
for bp in best_polytope
])
return np.atleast_1d(list(best_polytope)[0])
def find_closest_state(self, query_point, save_true_dynamics_path=False):
'''
Find the closest state from the query point to a given polytope
:param query_point:
:return: Tuple (closest_point, closest_point_is_self.state)
'''
distance = np.inf
closest_point = None
p_used = None
try:
#use AABB to upper bound distance
min_dmax = np.inf
for i, aabb in enumerate(self.aabb_list):
dmax = point_to_box_dmax(query_point, aabb)
if dmax < min_dmax:
min_dmax = dmax
for i, p in enumerate(self.polytope_list):
#ignore polytopes that are impossible
if point_to_box_distance(query_point,
self.aabb_list[i]) > min_dmax:
continue
d, proj = distance_point_polytope(p, query_point, ball='l2')
if d < distance:
distance = d
closest_point = proj
p_used = p
assert closest_point is not None
except TypeError:
closest_point = distance_point_polytope(self.polytope_list,
query_point,
ball='l2')[1]
p_used = self.polytope_list
if np.linalg.norm(closest_point - self.parent_state) < self.epsilon:
if save_true_dynamics_path:
return np.ndarray.flatten(closest_point), True, np.asarray([])
return np.ndarray.flatten(closest_point), True, np.asarray(
[self.parent_state,
np.ndarray.flatten(closest_point)])
if self.use_true_reachable_set and self.reachable_set_step_size:
#solve for the control input that leads to this state
current_linsys = self.sys.get_linearization(
self.parent_state) #FIXME: does mode need to be specified?
u = np.dot(np.linalg.pinv(current_linsys.B*self.reachable_set_step_size), \
(np.ndarray.flatten(closest_point)-np.ndarray.flatten(self.parent_state)-\
self.reachable_set_step_size*(np.dot(current_linsys.A, self.parent_state)+\
np.ndarray.flatten(current_linsys.c))))
u = np.ndarray.flatten(u)[0:self.sys.u.shape[0]]
#simulate nonlinear forward dynamics
state = self.parent_state
state_list = [self.parent_state]
for step in range(
int(self.reachable_set_step_size /
self.nonlinear_dynamic_step_size)):
try:
state = self.sys.forward_step(
u=np.atleast_1d(u),
linearlize=False,
modify_system=False,
step_size=self.nonlinear_dynamic_step_size,
return_as_env=False,
starting_state=state)
state_list.append(state)
except Exception as e:
print('Caught %s' % e)
return np.ndarray.flatten(closest_point), True, np.asarray(
[])
# print step,state
# print(state, closest_point)
if save_true_dynamics_path:
if len(state_list) <= 3:
print('Warning: short true dynamics path')
return np.ndarray.flatten(state), False, np.asarray(state_list)
else:
return np.ndarray.flatten(state), False, np.asarray(
[self.parent_state,
np.ndarray.flatten(state)])
else:
return np.ndarray.flatten(closest_point), False, np.asarray(
[self.parent_state,
np.ndarray.flatten(closest_point)])
