class BoardGraph(object):
def walk(self, board, size_limit=maxint):
pending_nodes = []
self.graph = DiGraph()
self.start = board.display(cropped=True)
self.graph.add_node(self.start)
pending_nodes.append(self.start)
self.min_domino_count = len(board.dominoes)
while pending_nodes:
if len(self.graph) >= size_limit:
raise GraphLimitExceeded(size_limit)
state = pending_nodes.pop()
board = Board.create(state, border=1)
dominoes = set(board.dominoes)
self.min_domino_count = min(self.min_domino_count, len(dominoes))
for domino in dominoes:
dx, dy = domino.direction
self.try_move(state, domino, dx, dy, pending_nodes)
self.try_move(state, domino, -dx, -dy, pending_nodes)
self.last = state
return set(self.graph.nodes())
def try_move(self, old_state, domino, dx, dy, pending_states):
try:
new_state = self.move(domino, dx, dy)
move = domino.describe_move(dx, dy)
if not self.graph.has_node(new_state):
# new node
self.graph.add_node(new_state)
pending_states.append(new_state)
self.graph.add_edge(old_state, new_state, move=move)
except BoardError:
pass
def move(self, domino, dx, dy):
""" Move a domino and calculate the new board state.
Afterward, put the board back in its original state.
@return: the new board state
@raise BoardError: if the move is illegal
"""
domino.move(dx, dy)
try:
board = domino.head.board
if not board.isConnected():
raise BoardError('Board is not connected.')
if board.hasLoner():
raise BoardError('Board has a lonely domino.')
return board.display(cropped=True)
finally:
domino.move(-dx, -dy)
class BoardGraph(object):
def __init__(self, board_class=Board):
self.graph = self.start = self.last = self.closest = None
self.min_domino_count = None
self.board_class = board_class
def walk(self, board, size_limit=maxsize):
pending_nodes = []
self.graph = DiGraph()
self.start = board.display(cropped=True)
self.graph.add_node(self.start)
pending_nodes.append(self.start)
self.last = self.start
while pending_nodes:
if len(self.graph) >= size_limit:
raise GraphLimitExceeded(size_limit)
state = pending_nodes.pop()
board = self.board_class.create(state, border=1)
for move, new_state in self.generate_moves(board):
if not self.graph.has_node(new_state):
# new node
self.graph.add_node(new_state)
pending_nodes.append(new_state)
self.graph.add_edge(state, new_state, move=move)
return set(self.graph.nodes())
def generate_moves(self, board):
""" Generate all moves from the board's current state.
:param Board board: the current state
:return: a generator of (state, move_description) tuples
"""
dominoes = set(board.dominoes)
domino_count = len(dominoes)
if self.min_domino_count is None or domino_count < self.min_domino_count:
self.min_domino_count = domino_count
self.last = board.display(cropped=True)
for domino in dominoes:
dx, dy = domino.direction
yield from self.try_move(domino, dx, dy)
yield from self.try_move(domino, -dx, -dy)
def try_move(self, domino, dx, dy):
try:
new_state = self.move(domino, dx, dy)
move = domino.describe_move(dx, dy)
yield move, new_state
except BadPositionError:
pass
def move(self, domino, dx, dy):
""" Move a domino and calculate the new board state.
Afterward, put the board back in its original state.
@return: the new board state
@raise BadPositionError: if the move is illegal
"""
domino.move(dx, dy)
try:
board = domino.head.board
if not board.isConnected():
raise BadPositionError('Board is not connected.')
if board.hasLoner():
raise BadPositionError('Board has a lonely domino.')
return board.display(cropped=True)
finally:
domino.move(-dx, -dy)
def get_solution(self, return_partial=False):
""" Find a solution from the graph of moves.
@param return_partial: If True, a partial solution will be returned if no
solution exists.
@return: a list of strings describing each move. Each string is two
digits describing the domino that moved plus a letter to show the
direction.
"""
solution = []
goal = self.closest if return_partial else self.last or ''
solution_nodes = shortest_path(self.graph, self.start, goal)
for i in range(len(solution_nodes) - 1):
source, target = solution_nodes[i:i + 2]
solution.append(self.graph[source][target]['move'])
return solution
def get_choice_counts(self):
solution_nodes = shortest_path(self.graph, self.start, self.last)
return [len(self.graph[node]) for node in solution_nodes[:-1]]
def get_average_choices(self):
choices = self.get_choice_counts()
return sum(choices) / float(len(choices)) if choices else maxsize
def get_max_choices(self):
choices = self.get_choice_counts()
return max(choices) if choices else maxsize
class BiasedTree(object):
"""
biased tree for prefix and suffix parts
"""
def __init__(self, workspace, geodesic, buchi, task, init_state,
init_label, init_angle, segment, para):
"""
initialization of the tree
:param workspace: workspace
:param buchi: buchi automaton
:param init_state: initial location of the robots
:param init_label: label generated by the initial location
:param segment: prefix or suffix part
:param para: parameters regarding biased-sampling method
"""
# parameters regarding workspace
self.workspace_instance = workspace
self.workspace = workspace.workspace
self.dim = len(self.workspace)
# self.regions = workspace.regions
self.obstacles = workspace.obs
self.node_landmark = Landmark()
self.node_landmark.update_from_workspace(workspace)
self.geodesic = geodesic
self.robot = buchi.number_of_robots
# parameters regarding task
self.buchi = buchi
self.task = task
self.accept = self.buchi.buchi_graph.graph['accept']
self.init = init_state
# initlizing the tree
self.biased_tree = DiGraph(type='PBA', init=self.init)
self.biased_tree.add_node(self.init, cost=0, label=init_label, \
angle=init_angle, lm=self.node_landmark, \
node_id=0)
self.node_count = 1
# parameters regarding TL-RRT* algorithm
self.goals = []
self.step_size = para['step_size']
self.segment = segment
self.lite = para['is_lite']
# size of the ball used in function near
uni_v = np.power(np.pi, self.robot * self.dim /
2) / math.gamma(self.robot * self.dim / 2 + 1)
self.gamma = np.ceil(
4 * np.power(1 / uni_v, 1. /
(self.dim * self.robot))) # unit workspace
# parameters regarding biased sampling
# group the nodes in the tree by the buchi state
self.group = dict()
self.add_group(self.init)
# select final buchi states
if self.segment == 'prefix':
self.b_final = self.buchi.buchi_graph.graph['accept'][0]
else:
self.b_final = self.buchi.buchi_graph.graph['accept']
self.min_dis = np.inf
self.q_min2final = []
self.not_q_min2final = []
self.update_min_dis2final_and_partition(self.init)
# probability of selecting q_p_closest
self.p_closest = para['p_closest']
# weight when selecting x_rand
self.y_rand = para['y_rand']
# threshold for collision avoidance
self.threshold = para['threshold']
# Updates landmark covariance when inside sensor range
self.update_covariance = para['update_covariance']
# sensor range in meters
self.sensor_range = para['sensor_range']
# sensor measurement noise
self.sensor_R = para['sensor_R']
# polygon obstacle for visibility-based method
polys = []
for poly in self.obstacles.values():
polys.append([
vg.Point(x[0], x[1]) for x in list(poly.exterior.coords)[:-1]
])
self.g = vg.VisGraph()
self.g.build(polys, status=False)
def trunc(self, i, value):
"""
limit the robot in the range of workspace
:param i: robot i, starting from 0
:param value: value to be adjusted
:return: adjusted value
"""
if value < 0:
return 0
elif value > self.workspace[i]:
return self.workspace[i]
else:
return value
def biased_sample(self):
"""
buchi guided biased sample
:return: sampled point x_rand, angles of robots, closest node
q_p_closest in terms of transitions, label of x_rand
"""
# sample nodes as q_p_closest from two partitioned sets
p_rand = np.random.uniform(0, 1, 1)
q_p_closest = None
if (p_rand <= self.p_closest
and len(self.q_min2final) > 0) or not self.not_q_min2final:
q_p_closest = sample_uniform_geometry(self.q_min2final)
elif p_rand > self.p_closest or not self.q_min2final:
q_p_closest = sample_uniform_geometry(self.not_q_min2final)
# find the reachable sets of buchi state of q_p_closest
reachable_q_b_closest = []
for b_state in self.buchi.buchi_graph.succ[q_p_closest[1]]:
if self.check_transition_b_helper(
self.biased_tree.nodes[q_p_closest]['label'],
self.buchi.buchi_graph.edges[(q_p_closest[1],
b_state)]['truth'],
self.buchi.buchi_graph.edges[(q_p_closest[1],
b_state)]['AP_keys']):
reachable_q_b_closest.append(b_state)
# if reachable_q_b_closest is empty
if not reachable_q_b_closest:
return [], [], [], [], []
# collect the buchi states in the reachable set of q_p_closest with minimum distance to the final state
b_min_from_q_b_closest = self.get_min2final_from_subset(
reachable_q_b_closest)
# collect the buchi states in the reachable set b_min_from_q_b_closest whose successors is 1 step less from
# the final state than the it is
reachable_decr = dict()
m_q_b_closest = []
for b_state in b_min_from_q_b_closest:
candidate = []
for succ in self.buchi.buchi_graph.succ[b_state]:
if self.buchi.min_length[(b_state, self.b_final)] - 1 == self.buchi.min_length[(succ, self.b_final)] \
or succ in self.buchi.buchi_graph.graph['accept']:
candidate.append(succ)
if candidate:
reachable_decr[b_state] = candidate
m_q_b_closest.append(b_state)
# if empty
if not m_q_b_closest:
return [], [], [], [], []
# sample q_b_min and q_b_decr
q_b_min = sample_uniform_geometry(m_q_b_closest)
q_b_decr = sample_uniform_geometry(reachable_decr[q_b_min])
# get the guarding symbol
truth = self.buchi.buchi_graph.edges[(q_b_min, q_b_decr)]['truth']
AP_truth = self.buchi.buchi_graph.edges[(q_b_min, q_b_decr)]['AP_keys']
avoid_targets = self.buchi.buchi_graph.edges[(q_b_min,
q_b_decr)]['avoid']
avoid_targets_2 = self.buchi.buchi_graph.edges[(
q_b_min, q_b_decr)]['avoid_self_loop']
x_rand = list(q_p_closest[0])
x_angle = self.biased_tree.nodes[q_p_closest]['angle'].copy()
"""get landmark state from q_p_closest and pass to below function"""
self.node_landmark = Landmark()
# self.node_landmark.update_from_landmark(self.biased_tree.nodes[q_p_closest]['lm'])
self.node_landmark = deepcopy(
self.biased_tree.nodes[q_p_closest]['lm'])
return self.buchi_guided_sample_by_truthvalue(
truth, AP_truth, avoid_targets, avoid_targets_2, x_rand,
q_p_closest, self.biased_tree.nodes[q_p_closest]['label'], x_angle,
q_b_decr)
def buchi_guided_sample_by_truthvalue(self, truth, AP_truth, avoid_targets,
avoid_targets_2, x_rand, q_p_closest,
x_label, x_angle, target_b_state):
"""
sample a point moving towards the region corresponding to the guarding symbol
:param truth: guarding symbol that enables the transition
:param q_p_closest: the node q_p_closest
:param x_rand: point to be sampled
:param x_label: label of position of q_p_closest
:return: sampled point x_rand, q_p_closest
"""
# x_angle = self.biased_tree.nodes[q_p_closest]['angle']
if truth == '1':
label = self.task.get_label_landmark(q_p_closest[0],
self.node_landmark)
return q_p_closest[0], x_angle, q_p_closest, label, target_b_state
else:
for key in truth:
# move towards the target position
found = False
for AP_key in x_label.keys():
if key in x_label[AP_key] and str(AP_key) in AP_truth:
found = True
if truth[key] and found == False:
pair = key.split('_') # region-robot pair
robot_index = int(pair[1]) - 1
orig_x_rand = x_rand[
robot_index] # save for further recover
orig_angle = x_angle[robot_index]
count = 0
while True:
x_rand[robot_index] = orig_x_rand # recover
count += 1
""" check distance with landmark and take random only if in sensor range"""
distance_from_lm = self.dist_from_landmark(
orig_x_rand, pair[0])
target, lm_pos = self.get_target(
orig_x_rand, pair[0], avoid_targets[robot_index],
avoid_targets_2[robot_index])
if distance_from_lm > self.sensor_range:
if np.random.uniform(
0, 1, 1) <= self.y_rand and count < 500:
x_rand[robot_index], x_angle[
robot_index] = self.sample_control_to_target(
orig_x_rand, orig_angle, target,
lm_pos)
for lm_key in self.node_landmark.landmark.keys(
):
if self.dist_from_landmark(
x_rand[robot_index], lm_key
) < self.sensor_range and self.update_covariance:
self.node_landmark.landmark[lm_key][
1] = ekf_update(
self.node_landmark.
landmark[lm_key][1], lm_pos,
x_rand[robot_index],
self.sensor_R)
self.node_landmark.generate_samples_for_lm(
lm_key)
else:
x_rand[robot_index], x_angle[
robot_index] = self.sample_control_random(
orig_x_rand, orig_angle)
for lm_key in self.node_landmark.landmark.keys(
):
if self.dist_from_landmark(
x_rand[robot_index], lm_key
) < self.sensor_range and self.update_covariance:
self.node_landmark.landmark[lm_key][
1] = ekf_update(
self.node_landmark.
landmark[lm_key][1], lm_pos,
x_rand[robot_index],
self.sensor_R)
self.node_landmark.generate_samples_for_lm(
lm_key)
else:
if np.random.uniform(0, 1, 1) <= 0.7:
x_rand[robot_index], x_angle[
robot_index] = self.sample_control_to_target(
orig_x_rand, orig_angle, target,
lm_pos)
for lm_key in self.node_landmark.landmark.keys(
):
if self.dist_from_landmark(
x_rand[robot_index], lm_key
) < self.sensor_range and self.update_covariance:
self.node_landmark.landmark[lm_key][
1] = ekf_update(
self.node_landmark.
landmark[lm_key][1], lm_pos,
x_rand[robot_index],
self.sensor_R)
self.node_landmark.generate_samples_for_lm(
lm_key)
else:
x_rand[robot_index], x_angle[
robot_index] = self.sample_control_random(
orig_x_rand, orig_angle)
for lm_key in self.node_landmark.landmark.keys(
):
if self.dist_from_landmark(
x_rand[robot_index], lm_key
) < self.sensor_range and self.update_covariance:
self.node_landmark.landmark[lm_key][
1] = ekf_update(
self.node_landmark.
landmark[lm_key][1], lm_pos,
x_rand[robot_index],
self.sensor_R)
self.node_landmark.generate_samples_for_lm(
lm_key)
# sampled point lies within obstacles
if 'o' in self.get_label(x_rand[robot_index]): continue
# collision avoidance
if self.collision_avoidance(x_rand, robot_index): break
label = self.task.get_label_landmark(x_rand, self.node_landmark)
return tuple(x_rand), x_angle, q_p_closest, label, target_b_state
def add_group(self, q_p):
"""
add q_p to the group within which all states have the same buchi state
:param q_p: a product state
"""
try:
self.group[q_p[1]].append(q_p)
except KeyError:
self.group[q_p[1]] = [q_p]
def get_min2final_from_subset(self, subset):
"""
collect the buchi state from the subset of nodes with minimum distance to the final state
:param subset: set of nodes
:return: list of buchi states with minimum distance to the final state
"""
l_min = np.inf
b_min = set()
for b_state in subset:
if self.buchi.min_length[(b_state, self.b_final)] < l_min:
l_min = self.buchi.min_length[(b_state, self.b_final)]
b_min = set([b_state])
elif self.buchi.min_length[(b_state, self.b_final)] == l_min:
b_min.add(b_state)
return b_min
def update_min_dis2final_and_partition(self, q_p_new):
"""
check whether q_p_new has the buchi component with minimum distance to the final state
if so, update the set b_min which collects the buchi states with minimum distance to the final state
and the set q_min2final which collects nodes in the tree with buchi states in b_min
:param q_p_new: new product state
"""
# smaller than the current nodes with minimum distance
if self.buchi.min_length[(q_p_new[1], self.b_final)] < self.min_dis:
self.min_dis = self.buchi.min_length[(q_p_new[1], self.b_final)]
self.not_q_min2final = self.not_q_min2final + self.q_min2final
self.q_min2final = [q_p_new]
# equivalent to
elif self.buchi.min_length[(q_p_new[1], self.b_final)] == self.min_dis:
self.q_min2final = self.q_min2final + [q_p_new]
# larger than
else:
self.not_q_min2final = self.not_q_min2final + [q_p_new]
def get_target(self, init, target, avoid_targets, avoid_targets_2):
"""
find the second vertex in the shortest path from initial point to the target region
:param init: initial point
:param target: target labeled region
:return: the second vertex
"""
if target[0] == 'c':
landmark_id = np.argmax(
self.workspace_instance.classes[:, (int(target[1:]) - 1)]) + 1
target = 'l' + str(landmark_id)
tg = self.node_landmark.landmark[target][0]
# shortest = self.g.shortest_path(vg.Point(init[0], init[1]), vg.Point(tg[0], tg[1]))
# start = datetime.datetime.now()
avoid = avoid_targets.copy()
for i in range(len(avoid_targets_2)):
avoid.append(avoid_targets_2[i])
waypoint = self.geodesic.get_geodesic_target(init, tg,
self.node_landmark, avoid)
# print(go_to)
# NBA_time = (datetime.datetime.now() - start).total_seconds()
# print('Time for getting path: {0:.4f} s'.format(NBA_time))
return waypoint, tg
# return shortest[1].x, shortest[1].y
def get_truncated_normal(self, mean=0, sd=1, low=0, upp=10):
"""
truncated normal distribution
:param mean: mean value
:param sd: standard deviation
:param low: lower bound of the random variable
:param upp: upper bound of the random variable
:return: value of the random variable
"""
return truncnorm((low - mean) / sd, (upp - mean) / sd,
loc=mean,
scale=sd)
# def gaussian_guided_towards_target(self, x, target):
# """
# calculate new point x_rand guided by the target
# distance and angle follow the gaussian distribution
# :param x: initial point
# :param target: target point
# :return: new point x_rand
# """
# d = 0.3
# angle = np.random.normal(0, np.pi/12/3/3, 1) + np.arctan2(target[1] - x[1], target[0] - x[0])
# x_rand = np.add(x, np.append(d * np.cos(angle), d * np.sin(angle)))
# x_rand = [self.trunc(i, x_rand_i) for i, x_rand_i in enumerate(x_rand)]
# return tuple(x_rand)
def sample_control_to_target(self, x, orig_angle, target, lm_pos):
"""
calculate new point x_rand guided by the target
distance and angle follow the gaussian distribution
:param x: initial point
:param target: target point
:return: new point x_rand
"""
if x == target:
return x, orig_angle
"""Continuous dynamics"""
# d = 4
# angle = np.random.normal(0, np.pi/12/3/3, 1) + np.arctan2(target[1] - x[1], target[0] - x[0])
# # angle = 0.4*orig_angle + 0.6*angle
# diff = angle-orig_angle
# if diff>np.pi:
# diff = diff - 2*np.pi
# elif diff<-np.pi:
# diff = 2*np.pi + diff
# angle = orig_angle + 0.4*diff
# if angle>np.pi:
# angle = angle - 2*np.pi
# elif angle <-np.pi:
# angle = 2*np.pi + angle
# x_rand = np.add(x, np.append(d * np.cos(angle), d * np.sin(angle)))
# x_rand = [self.trunc(i, x_rand_i) for i, x_rand_i in enumerate(x_rand)]
"""discrete dynamics"""
tau = 0.5
if math.sqrt((lm_pos[1] - x[1])**2 + (lm_pos[0] - x[0])**2) < 10:
dd = 3
else:
dd = 10
d = dd * tau
angle = np.arctan2(target[1] - x[1], target[0] -
x[0]) # + np.random.normal(0, np.pi/12/3/3, 1)
x_rand = np.add(x, np.append(d * np.cos(angle), d * np.sin(angle)))
x_rand = [self.trunc(i, x_rand_i) for i, x_rand_i in enumerate(x_rand)]
"""Brent dynamics"""
# d=5
# dd=5
# curTheta=orig_angle
# u = np.array([0,45,-45,90,-90,135,-135,180,-180])*np.pi/180
# uu = u + orig_angle
# uu[uu>np.pi] = uu[uu>np.pi] - 2*np.pi
# uu[uu<-np.pi] = uu[uu<-np.pi] + 2*np.pi
# angle = np.arctan2(target[1] - x[1], target[0] - x[0])
# index = np.where(abs((uu-angle))==min(abs(uu-angle)))
# uuu=u[index[0][0]]
# if abs(tau*uuu)<0.001:
# x_rand = np.add(x,np.append(d*np.cos(curTheta+tau*uuu/2),d*np.sin(curTheta+tau*uuu/2)))
# x_rand = [self.trunc(i, x_rand_i) for i, x_rand_i in enumerate(x_rand)]
# else:
# x_rand = np.add(x, np.append((dd/uuu)*(np.sin(curTheta+tau*uuu) - np.sin(curTheta)), (dd/uuu)*(np.cos(curTheta) - np.cos(curTheta+tau*uuu))))
# x_rand = [self.trunc(i, x_rand_i) for i, x_rand_i in enumerate(x_rand)]
return tuple(x_rand), angle
def sample_control_random(self, x, orig_angle):
"""
calculate new point x_rand guided by the target
distance and angle follow the gaussian distribution
:param x: initial point
:param target: target point
:return: new point x_rand
"""
tau = 0.5
dd = 5
d = dd * tau
# angle = np.random.uniform(-np.pi, np.pi)
u = np.array([0, 45, -45, 90, -90, 135, -135, 180, -180
]) * tau * np.pi / 180
angle = np.random.choice(u) + orig_angle
x_rand = np.add(x, np.append(d * np.cos(angle), d * np.sin(angle)))
x_rand = [self.trunc(i, x_rand_i) for i, x_rand_i in enumerate(x_rand)]
return tuple(x_rand), angle
def collision_avoidance(self, x, robot_index):
"""
check whether robots with smaller index than robot_index collide with the robot of index robot_index
:param x: position of robots
:param robot_index: index of the specific robot
:return: true if collision free
"""
for i in range(len(x)):
if i != robot_index:
if np.fabs(x[i][0] - x[robot_index][0]) <= self.threshold and \
np.fabs(x[i][1] - x[robot_index][1]) <= self.threshold:
return False
return True
def nearest(self, x_rand):
"""
find the nearest class of vertices in the tree
:param: x_rand randomly sampled point form: single point ()
:return: nearest class of vertices form: single point ()
"""
min_dis = math.inf
q_p_nearest = []
for node in self.biased_tree.nodes:
x = self.mulp2single(node[0])
dis = np.linalg.norm(np.subtract(x_rand, x))
if dis < min_dis:
q_p_nearest = [node]
min_dis = dis
elif dis == min_dis:
q_p_nearest.append(node)
return q_p_nearest
def steer(self, x_rand, x_nearest):
"""
steer
:param: x_rand randomly sampled point form: single point ()
:param: x_nearest nearest point in the tree form: single point ()
:return: new point single point ()
"""
if np.linalg.norm(np.subtract(x_rand, x_nearest)) <= self.step_size:
return x_rand
else:
return tuple(
np.asarray(x_nearest) + self.step_size *
(np.subtract(x_rand, x_nearest)) /
np.linalg.norm(np.subtract(x_rand, x_nearest)))
def extend(self, q_new, x_angle, near_nodes, label, obs_check,
target_b_state):
"""
add the new sate q_new to the tree
:param: q_new: new state
:param: near_nodes: near state
:param: obs_check: check the line connecting two points are inside the freespace
:return: the tree after extension
"""
added = False
cost = np.inf
q_min = ()
# loop over all nodes in near_nodes
for node in near_nodes:
# if q_new != node and obs_check[(q_new[0], node[0])] and \
# self.check_transition_b(node[1], self.biased_tree.nodes[node]['label'], q_new[1]):
if q_new != node and obs_check[(q_new[0], node[0])] and \
self.check_transition_b(node[1], label, q_new[1]):
c = self.biased_tree.nodes[node]['cost'] \
+ np.linalg.norm(np.subtract(self.mulp2single(q_new[0]), self.mulp2single(node[0])))
if c < cost:
added = True
q_min = node
cost = c
if added and not self.biased_tree.has_node(q_new):
self.biased_tree.add_node(q_new,
cost=cost,
label=label,
angle=x_angle,
lm=self.node_landmark,
node_id=self.node_count,
target=target_b_state)
self.node_count += 1
self.biased_tree.add_edge(q_min, q_new)
self.add_group(q_new)
self.update_min_dis2final_and_partition(q_new)
if self.segment == 'prefix' and q_new[1] in self.accept:
# q_n = list(list(self.biased_tree.pred[q_new].keys())[0])
# cost = self.biased_tree.nodes[tuple(q_n)]['cost']
# label = self.biased_tree.nodes[tuple(q_n)]['label']
# q_n[1] = q_new[1]
# q_n = tuple(q_n)
# self.biased_tree.add_node(q_n, cost=cost, label=label)
# self.biased_tree.add_edge(q_min, q_n)
# self.add_group(q_n)
# self.update_min_dis2final_and_partition(q_n)
# self.goals.append(q_n)
self.goals.append(q_new)
if self.segment == 'suffix' and self.init[1] == q_new[1]:
self.goals.append(q_new)
return added
def rewire(self, q_new, near_nodes, obs_check):
"""
:param: q_new: new state
:param: near_nodes: states returned near
:param: obs_check: check whether obstacle-free
:return: the tree after rewiring
"""
for node in near_nodes:
if obs_check[(q_new[0], node[0])] \
and self.check_transition_b(q_new[1], self.biased_tree.nodes[q_new]['label'], node[1]):
c = self.biased_tree.nodes[q_new]['cost'] \
+ np.linalg.norm(np.subtract(self.mulp2single(q_new[0]), self.mulp2single(node[0])))
delta_c = self.biased_tree.nodes[node]['cost'] - c
# update the cost of node in the subtree rooted at the rewired node
if delta_c > 0:
self.biased_tree.remove_edge(
list(self.biased_tree.pred[node].keys())[0], node)
self.biased_tree.add_edge(q_new, node)
edges = dfs_labeled_edges(self.biased_tree, source=node)
for _, v, d in edges:
if d == 'forward':
self.biased_tree.nodes[v][
'cost'] = self.biased_tree.nodes[v][
'cost'] - delta_c
def near(self, x_new):
"""
find the states in the near ball
:param x_new: new point form: single point
:return: p_near: near state, form: tuple (mulp, buchi)
"""
near_nodes = []
radius = min(
self.gamma * np.power(
np.log(self.biased_tree.number_of_nodes() + 1) /
self.biased_tree.number_of_nodes(), 1. /
(self.dim * self.robot)), self.step_size)
for node in self.biased_tree.nodes:
if np.linalg.norm(np.subtract(x_new, self.mulp2single(
node[0]))) <= radius:
near_nodes.append(node)
return near_nodes
def obstacle_check(self, near_node, x_new, label):
"""
check whether line from x_near to x_new is obstacle-free
:param near_node: nodes returned by near function
:param x_new: new position component
:param label: label of x_new
:return: a dictionary indicating whether the line connecting two points are obstacle-free
"""
obs_check = {}
checked = set()
for node in near_node:
# whether the position component of nodes has been checked
if node[0] in checked:
continue
checked.add(node[0])
obs_check[(x_new, node[0])] = True
flag = True # indicate whether break and jump to outer loop
for r in range(self.robot):
# the line connecting two points crosses an obstacle
for (obs, boundary) in iter(self.obstacles.items()):
if LineString([Point(node[0][r]),
Point(x_new[r])]).intersects(boundary):
obs_check[(x_new, node[0])] = False
flag = False
break
# no need to check further
if not flag:
break
return obs_check
def get_label(self, x):
"""
generating the label of position component
:param x: position
:return: label
"""
point = Point(x)
# whether x lies within obstacle
for (obs, boundary) in iter(self.obstacles.items()):
if point.within(boundary):
return obs
return ''
def check_transition_b(self, q_b, x_label, q_b_new):
"""
check whether q_b -- x_label ---> q_b_new
:param q_b: buchi state
:param x_label: label of x
:param q_b_new: buchi state
:return True if satisfied
"""
b_state_succ = self.buchi.buchi_graph.succ[q_b]
# q_b_new is not the successor of b_state
if q_b_new not in b_state_succ:
return False
# check whether label of x enables the transition
truth = self.buchi.buchi_graph.edges[(q_b, q_b_new)]['truth']
AP_truth = self.buchi.buchi_graph.edges[(q_b, q_b_new)]['AP_keys']
if self.check_transition_b_helper(x_label, truth, AP_truth):
return True
return False
def check_transition_b_helper(self, x_label, truth, AP_truth):
"""
check whether transition enabled with current generated label
:param x_label: label of the current position
:param truth: symbol enabling the transition
:return: true or false
"""
if truth == '1':
return True
# all true propositions should be satisdied
true_label = [
true_label for true_label in truth.keys() if truth[true_label]
]
for label in true_label:
found = False
for key in x_label.keys():
if label in x_label[key] and str(key) in AP_truth:
found = True
if found == False:
return False
# all fasle propositions should not be satisfied
false_label = [
false_label for false_label in truth.keys()
if not truth[false_label]
]
for label in false_label:
found = False
for key in x_label.keys():
if label in x_label[key]:
found = True
if found == True:
return False
return True
def find_path(self, goals):
"""
find the path backwards
:param goals: found all goal states
:return: the path leading to the goal state and the corresponding cost
"""
paths = OrderedDict()
nodes = []
cov = []
targets = []
for i in range(len(goals)):
goal = goals[i]
path = [goal]
s = goal
nodes.insert(0, self.biased_tree.nodes[s]['node_id'])
cov.insert(0, self.biased_tree.nodes[s]['lm'])
targets.insert(0, self.biased_tree.nodes[s]['target'])
targets.insert(0, self.biased_tree.nodes[s]['target'])
# print(self.biased_tree.nodes[s]['node_id'])
while s != self.init:
s = list(self.biased_tree.pred[s].keys())[0]
path.insert(0, s)
nodes.insert(0, self.biased_tree.nodes[s]['node_id'])
cov.insert(0, self.biased_tree.nodes[s]['lm'])
# if s==self.init:
# targets.insert(0,targets[0])
# else:
if s != self.init:
targets.insert(0, self.biased_tree.nodes[s]['target'])
paths[i] = [self.biased_tree.nodes[goal]['cost'], path]
return paths, nodes, cov, targets
def mulp2single(self, point):
"""
convert a point, which in the form of a tuple of tuple ((),(),(),...) to point in the form of a flat tuple
:param point: point((position of robot 1), (position of robot2), (), ...)
:return: point (position of robot1, position of robot2, ...)
"""
return tuple([p for r in point for p in r])
def single2mulp(self, point):
"""
convert a point in the form of flat tuple to point in the form of a tuple of tuple ((),(),(),...)
:param point: point (position of robot1, position of robot2, ...)
:return: point((position of robot 1), (position of robot2), (), ...)
"""
mp = [
point[i * self.dim:(i + 1) * self.dim] for i in range(self.robot)
]
return tuple(mp)
def dist_from_landmark(self, x, lm_id):
"""
Parameters
----------
x : robot position
lm_id : landmark id (eg. l6)
Returns
-------
Distance between landmark and robot
"""
lm_pos = self.node_landmark.landmark[lm_id][0]
return math.sqrt((x[0] - lm_pos[0])**2 + (x[1] - lm_pos[1])**2)
