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)