def is_correctly_generated(graph: DiGraph, min_args, max_args) -> bool: """ Check if a graph is correctly generated """ # no empty graphs if graph.number_of_nodes() == 0 or graph.number_of_edges() == 0: return False # no incorrectly generated graphs if not min_args <= len(graph.nodes) <= max_args: return False # no graphs with isolated subgraphs if nx.number_connected_components(graph.to_undirected()) > 1: return False return True
class tree(object): """ construction of prefix and suffix tree """ def __init__(self, ts, buchi_graph, init, step_size, base=1e3): """ :param ts: transition system :param buchi_graph: Buchi graph :param init: product initial state """ self.robot = 1 self.goals = [] self.ts = ts self.buchi_graph = buchi_graph self.init = init self.step_size = step_size self.dim = len(self.ts['workspace']) 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 self.tree = DiGraph(type='PBA', init=init) label = self.label(init[0]) if label != '': label = label + '_' + str(1) # accepting state before current node acc = set() if 'accept' in init[1]: acc.add(init) self.tree.add_node(init, cost=0, label=label, acc=acc) self.search_goal(init, label, acc) # already used skilles self.used = set() self.base = base def sample(self): """ sample point from the workspace :return: sampled point, tuple """ x_rand = [] for i in range(self.dim): x_rand.append(uniform(0, self.ts['workspace'][i])) return tuple(x_rand) 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_nearest = [] for vertex in self.tree.nodes: x_vertex = vertex[0] dis = np.linalg.norm(np.subtract(x_rand, x_vertex)) if dis < min_dis: q_nearest = list() q_nearest.append(vertex) min_dis = dis elif dis == min_dis: q_nearest.append(vertex) return q_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 acpt_check(self, q_min, q_new): """ check the accepting state in the patg leading to q_new :param q_min: :param q_new: :return: """ changed = False acc = set(self.tree.nodes[q_min]['acc']) # copy if 'accept' in q_new[1]: acc.add(q_new) # print(acc) changed = True return acc, changed def search_goal(self, q_new, label_new, acc): """ whether q_new can connect to point before acc :param q_new: :param label_new: :param acc: :return: """ for ac in acc: # connect to path leading to accepting state, including accepting state path = self.findpath([ac])[0][1] for point in path: if list(self.obs_check([q_new], point[0], self.tree.nodes[point]['label']).values())[0] \ and self.checkTranB(q_new[1], label_new, point[1]): self.goals.append( (q_new, point, ac)) # endpoint, middle point, accepting point def extend(self, q_new, near_v, label, obs_check, succ_list): """ :param: q_new: new state form: tuple (mulp, buchi) :param: near_v: near state form: tuple (mulp, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :param: succ: list of successor of the root :return: extending the tree """ added = 0 cost = np.inf q_min = () for near_vertex in near_v: if near_vertex in succ_list: # do not extend if there is a corresponding root continue if q_new != near_vertex and obs_check[(q_new[0], near_vertex[0])] \ and self.checkTranB(near_vertex[1], self.tree.nodes[near_vertex]['label'], q_new[1]): c = self.tree.nodes[near_vertex]['cost'] + \ np.linalg.norm(np.subtract(q_new[0], near_vertex[0])) if c < cost: added = 1 q_min = near_vertex cost = c if added == 1: self.tree.add_node(q_new, cost=cost, label=label) self.tree.nodes[q_new]['acc'] = set( self.acpt_check(q_min, q_new)[0]) self.tree.add_edge(q_min, q_new) # self.search_goal(q_new, label, self.tree.nodes[q_new]['acc']) return added def rewire(self, q_new, near_v, obs_check): """ :param: q_new: new state form: tuple (mul, buchi) :param: near_v: near state form: tuple (mul, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :return: rewiring the tree """ for near_vertex in near_v: if obs_check[(q_new[0], near_vertex[0])] \ and self.checkTranB(q_new[1], self.tree.nodes[q_new]['label'], near_vertex[1]): c = self.tree.nodes[q_new]['cost'] \ + np.linalg.norm(np.subtract(q_new[0], near_vertex[0])) delta_c = self.tree.nodes[near_vertex]['cost'] - c # update the cost of node in the subtree rooted at near_vertex if delta_c > 0: # self.tree.nodes[near_vertex]['cost'] = c self.tree.remove_edge( list(self.tree.pred[near_vertex].keys())[0], near_vertex) self.tree.add_edge(q_new, near_vertex) edges = dfs_labeled_edges(self.tree, source=near_vertex) acc, changed = self.acpt_check(q_new, near_vertex) self.tree.nodes[near_vertex]['acc'] = set(acc) for u, v, d in edges: if d == 'forward': self.tree.nodes[v][ 'cost'] = self.tree.nodes[v]['cost'] - delta_c if changed: self.tree.nodes[v]['acc'] = set( self.acpt_check(u, v)[0]) # copy # better to research the goal but abandon the implementation 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) """ p_near = [] r = min( self.gamma * np.power( np.log(self.tree.number_of_nodes() + 1) / self.tree.number_of_nodes(), 1. / (self.dim * self.robot)), self.step_size) # r = self.step_size for vertex in self.tree.nodes: if np.linalg.norm(np.subtract(x_new, vertex[0])) <= r: p_near.append(vertex) return p_near def obs_check(self, q_near, x_new, label): """ check whether obstacle free along the line from x_near to x_new :param q_near: states in the near ball, tuple (mulp, buchi) :param x_new: new state form: multiple point :param label: label of x_new :param stage: regular stage or final stage, deciding whether it's goal state :return: dict (x_near, x_new): true (obs_free) """ obs_check_dict = {} checked = set() for x in q_near: if x[0] in checked: continue checked.add(x[0]) obs_check_dict[(x_new, x[0])] = True # the line connecting two points crosses an obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if LineString([Point(x[0]), Point(x_new)]).intersects(boundary): obs_check_dict[(x_new, x[0])] = False break for (region, boundary) in iter(self.ts['region'].items()): if LineString([Point(x[0]), Point(x_new)]).intersects(boundary) \ and region + '_' + str(1) != label \ and region + '_' + str(1) != self.tree.nodes[x]['label']: # if stage == 'reg' or (stage == 'final' and region in self.no): obs_check_dict[(x_new, x[0])] = False break return obs_check_dict def label(self, x): """ generating the label of position state :param x: position :return: label """ point = Point(x) # whether x lies within obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if point.within(boundary): return obs # whether x lies within regions for (region, boundary) in iter(self.ts['region'].items()): if point.within(boundary): return region # x lies within unlabeled region return '' def checkTranB(self, b_state, x_label, q_b_new): """ decide valid transition, whether b_state --L(x)---> q_b_new Algorithm2 in Chapter 2 Motion and Task Planning :param b_state: buchi state :param x_label: label of x :param q_b_new buchi state :return True satisfied """ b_state_succ = self.buchi_graph.succ[b_state] # q_b_new is not the successor of b_state if q_b_new not in b_state_succ: return False truth = self.buchi_graph.edges[(b_state, q_b_new)]['truth'] if self.t_satisfy_b_truth(x_label, truth): return True return False def t_satisfy_b_truth(self, x_label, truth): """ check whether transition enabled under current label :param x_label: current label :param truth: truth value making transition enabled :return: true or false """ if truth == '1': return True true_label = [ truelabel for truelabel in truth.keys() if truth[truelabel] ] for label in true_label: if label not in x_label: return False false_label = [ falselabel for falselabel in truth.keys() if not truth[falselabel] ] for label in false_label: if label in x_label: return False return True def findpath(self, goals): """ find the path backwards :param goals: goal state :return: dict path : cost """ paths = OrderedDict() for i in range(len(goals)): goal = goals[i] path = [goal] s = goal while s != self.init: s = list(self.tree.pred[s].keys())[0] if s == path[0]: print("loop") path.insert(0, s) paths[i] = [self.tree.nodes[goal]['cost'], path] return paths
class tree(object): """ construction of prefix and suffix tree """ def __init__(self, n_robot, acpt, ts, buchi_graph, init, seg, step_size, no): """ :param acpt: accepting state :param ts: transition system :param buchi_graph: Buchi graph :param init: product initial state """ self.robot = n_robot self.acpt = acpt self.goals = [] self.ts = ts self.buchi_graph = buchi_graph self.init = init self.seg = seg self.step_size = step_size self.dim = len(self.ts['workspace']) uni_ball = [ 1, 2, 3.142, 4.189, 4.935, 5.264, 5.168, 4.725, 4.059, 3.299, 2.550 ] # uni_v = uni_ball[self.robot*self.dim] 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 self.tree = DiGraph(type='PBA', init=init) self.group = dict() label = [] for i in range(self.robot): l = self.label(init[0][i]) # exists one sampled point lies within obstacles if l != '': l = l + '_' + str(i + 1) label.append(l) self.tree.add_node(init, cost=0, label=label) self.add_group(init) # probability self.p = 0.9 # threshold for collision avoidance self.threshold = 0.02 # polygon obstacle polys = [[ vg.Point(0.4, 1.0), vg.Point(0.4, 0.7), vg.Point(0.6, 0.7), vg.Point(0.6, 1.0) ], [ vg.Point(0.3, 0.2), vg.Point(0.3, 0.0), vg.Point(0.7, 0.0), vg.Point(0.7, 0.2) ]] self.g = vg.VisGraph() self.g.build(polys, status=False) # region that has ! preceding it self.no = no def add_group(self, q_state): """ group nodes with same buchi state :param q_state: new state added to the tree """ try: self.group[q_state[1]].append(q_state) except KeyError: self.group[q_state[1]] = [q_state] def min2final(self, min_qb_dict, b_final, cand): """ collects the buchi state in the tree with minimum distance to the final state :param min_qb_dict: dict :param b_final: feasible final state :return: list of buchi states in the tree with minimum distance to the final state """ l_min = np.inf b_min = [] for b_state in cand: if min_qb_dict[(b_state, b_final)] < l_min: l_min = min_qb_dict[(b_state, b_final)] b_min = [b_state] elif min_qb_dict[(b_state, b_final)] == l_min: b_min.append(b_state) return b_min def all2one(self, b_min): """ partition nodes into 2 groups :param b_min: buchi states with minimum distance to the finals state :return: 2 groups """ q_min2final = [] q_minNot2final = [] for b_state in self.group.keys(): if b_state in b_min: q_min2final = q_min2final + self.group[b_state] else: q_minNot2final = q_minNot2final + self.group[b_state] return q_min2final, q_minNot2final def get_truncated_normal(self, mean=0, sd=1, low=0, upp=10): return truncnorm((low - mean) / sd, (upp - mean) / sd, loc=mean, scale=sd) def trunc(self, value): if value < 0: return 0 elif value > 1: return 1 else: return value def collision_avoidance(self, x, index): """ check whether any robots are collision-free from index-th robot :param x: all robots :param index: index-th robot :return: true collision free """ for i in range(len(x)): if i != index and np.linalg.norm(np.subtract( x[i], x[index])) <= self.threshold: return False return True def target(self, init, target, regions): """ find the closest vertex in the short path from init to target :param init: inital point :param target: target labeled region :param regions: regions :return: closest vertex """ tg = regions[((target, 'b'))][:2] shortest = self.g.shortest_path(vg.Point(init[0], init[1]), vg.Point(tg[0], tg[1])) return (shortest[1].x, shortest[1].y) def gaussian_guided(self, x, target): """ calculate new point following gaussian dist guided by the target :param x: mean point :param target: target point :return: new point """ # print(min(self.gamma * np.power(np.log(self.tree.number_of_nodes()+1)/self.tree.number_of_nodes(),1./(self.dim*self.robot)), self.step_size)/3) # d = self.get_truncated_normal(0, min(self.gamma * np.power(np.log(self.tree.number_of_nodes()+1)/self.tree.number_of_nodes(),1./(self.dim*self.robot)), self.step_size)/3, 0, np.inf) # d = self.get_truncated_normal(0, self.step_size/3, 0, np.inf) d = self.get_truncated_normal(0, 1 / 3, 0, np.inf) # d = self.get_truncated_normal(0, 1, 0, np.inf) d = d.rvs() # # print('d=',d) # if np.random.uniform(0,1,1) <= self.p: # angle = np.random.uniform(-np.pi/2, np.pi/2, 1) + np.arctan2(center[1]-x[1], center[0]-x[0]) # else: # angle = np.random.uniform(np.pi/2, 3*np.pi/2, 1) + np.arctan2(center[1]-x[1], center[0]-x[0]) angle = np.random.normal(0, np.pi / 12 / 3 / 3, 1) + np.arctan2( target[1] - x[1], target[0] - x[0]) # angle = 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(x) for x in x_rand] return tuple(x_rand) def gaussian_unguided(self, x): """ :param x: :return: """ d = self.get_truncated_normal( 0, min( self.gamma * np.power( np.log(self.tree.number_of_nodes() + 1) / self.tree.number_of_nodes(), 1. / (self.dim * self.robot)), self.step_size) / 3, 0) d = d.rvs() angle = np.random.uniform(-np.pi, np.pi, 1) x_rand = np.add(x, np.append(d * np.cos(angle), d * np.sin(angle))) return tuple([self.trunc(x) for x in x_rand]) def buchi_guided_sample_by_label(self, x_rand, b_label, x_label, regions): if b_label.strip().strip('(').strip(')') == '1': return [] # not or be in some place else: # label of current position blabel = b_label.split('||')[0] # blabel = random.choice(b_label.split('||')) atomic_label = blabel.split('&&') for a in atomic_label: # a = a.strip().strip('(').strip(')') a = a.strip().strip('(').strip(')').split('or')[0].strip() # if in wrong position, sample randomly if '!' in a: if a[1:] in x_label: xi_rand = [] for i in range(self.dim): xi_rand.append(uniform(0, self.ts['workspace'][i])) ind = int(a[1:].split('_')[1]) - 1 x_rand[ind] = tuple(xi_rand) else: # move towards target position if not a in x_label: ind = a.split('_') weight = 0.8 if np.random.uniform(0, 1, 1) <= weight: tg = self.target(x_rand[int(ind[1]) - 1], ind[0], regions) x_rand[int(ind[1]) - 1] = self.gaussian_guided( x_rand[int(ind[1]) - 1], tg) else: xi_rand = [] for i in range(self.dim): xi_rand.append( uniform(0, self.ts['workspace'][i])) x_rand[int(ind[1]) - 1] = tuple(xi_rand) return x_rand def buchi_guided_sample_by_truthvalue(self, truth, x_rand, q_rand, x_label, regions): """ sample guided by truth value :param truth: the value making transition occur :param x_rand: random selected node :param x_label: label of x_rand :param regions: regions :return: new sampled point """ if truth == '1': return [], [] # not or be in some place else: for key in truth: # if in wrong position, sample randomly # if not truth[key] and key in x_label: # xi_rand = [] # for i in range(self.dim): # xi_rand.append(uniform(0, self.ts['workspace'][i])) # ind = key.split('_') # x_rand[int(ind[1]) - 1] = tuple(xi_rand) # elif truth[key]: # # move towards target position # if not key in x_label: # ind = key.split('_') # weight = 1 # if np.random.uniform(0, 1, 1) <= weight: # tg = self.target(x_rand[int(ind[1]) - 1], ind[0], regions) # # tg = self.target(orig_x_rand, ind[0], regions) # x_rand[int(ind[1]) - 1] = self.gaussian_guided(x_rand[int(ind[1]) - 1], tg) # else: # xi_rand = [] # for i in range(self.dim): # xi_rand.append(uniform(0, self.ts['workspace'][i])) # x_rand[int(ind[1]) - 1] = tuple(xi_rand) ind = key.split('_') orig_x_rand = x_rand[int(ind[1]) - 1] # save while 1: x_rand[int(ind[1]) - 1] = orig_x_rand # recover # if in wrong position, sample randomly if not truth[key] and key in x_label: xi_rand = [] for i in range(self.dim): xi_rand.append(uniform(0, self.ts['workspace'][i])) # ind = key.split('_') x_rand[int(ind[1]) - 1] = tuple(xi_rand) elif truth[key]: # move towards target position if not key in x_label: # ind = key.split('_') weight = 0.8 if np.random.uniform(0, 1, 1) <= weight: # tg = self.target(x_rand[int(ind[1]) - 1], ind[0], regions) tg = self.target(orig_x_rand, ind[0], regions) x_rand[int(ind[1]) - 1] = self.gaussian_guided( orig_x_rand, tg) else: xi_rand = [] for i in range(self.dim): xi_rand.append( uniform(0, self.ts['workspace'][i])) x_rand[int(ind[1]) - 1] = tuple(xi_rand) else: break if self.collision_avoidance(x_rand, int(ind[1]) - 1): break # x_rand x_nearest return self.mulp2sglp(x_rand), q_rand # return x_rand def sample(self, buchi_graph, min_qb_dict, regions): """ sample point from the workspace :return: sampled point, tuple """ if self.seg == 'pre': b_final = buchi_graph.graph['accept'][np.random.randint( 0, len(buchi_graph.graph['accept']) )] # feasible final buchi state else: b_final = buchi_graph.graph['accept'] # collects the buchi state in the tree with minimum distance to the final state b_min = self.min2final(min_qb_dict, b_final, self.group.keys()) # partition of nodes q_min2final, q_minNot2final = self.all2one(b_min) # sample random nodes p_rand = np.random.uniform(0, 1, 1) if (p_rand <= self.p and len(q_min2final) > 0) or not q_minNot2final: q_rand = q_min2final[np.random.randint(0, len(q_min2final))] elif p_rand > self.p or not q_min2final: q_rand = q_minNot2final[np.random.randint(0, len(q_minNot2final))] # find feasible succssor of buchi state in q_rand Rb_q_rand = [] x_label = [] for i in range(self.robot): l = self.label(q_rand[0][i]) if l != '': l = l + '_' + str(i + 1) x_label.append(l) for b_state in buchi_graph.succ[q_rand[1]]: # if self.t_satisfy_b(x_label, buchi_graph.edges[(q_rand[1], b_state)]['label']): if self.t_satisfy_b_truth( x_label, buchi_graph.edges[(q_rand[1], b_state)]['truth']): Rb_q_rand.append(b_state) # if empty if not Rb_q_rand: return Rb_q_rand, Rb_q_rand # collects the buchi state in the reachable set of qb_rand with minimum distance to the final state b_min = self.min2final(min_qb_dict, b_final, Rb_q_rand) # collects the buchi state in the reachable set of b_min with distance to the final state equal to that of b_min - 1 decr_dict = dict() for b_state in b_min: decr = [] for succ in buchi_graph.succ[b_state]: if min_qb_dict[(b_state, b_final)] - 1 == min_qb_dict[( succ, b_final)] or succ in buchi_graph.graph['accept']: decr.append(succ) decr_dict[b_state] = decr M_cand = [ b_state for b_state in decr_dict.keys() if decr_dict[b_state] ] # if empty if not M_cand: return M_cand, M_cand # sample b_min and b_decr b_min = M_cand[np.random.randint(0, len(M_cand))] b_decr = decr_dict[b_min][np.random.randint(0, len(decr_dict[b_min]))] # b_label = buchi_graph.edges[(b_min, b_decr)]['label'] # x_rand = list(q_rand[0]) # # return self.buchi_guided_sample_by_label(x_rand, b_label, x_label, regions) truth = buchi_graph.edges[(b_min, b_decr)]['truth'] x_rand = list(q_rand[0]) return self.buchi_guided_sample_by_truthvalue(truth, x_rand, q_rand, x_label, regions) # x_rand x_nearest # return self.mulp2sglp(x_rand), self.mulp2sglp(q_rand[0]) # return x_rand # def nearest(self, x_rand): # """ # find the nearest vertex in the tree # :param: x_rand randomly sampled point form: single point () # :return: nearest vertex form: single point () # """ # min_dis = math.inf # x_nearest = x_rand # for vertex in self.tree.nodes: # x_vertex = self.mulp2sglp(vertex[0]) # dis = np.linalg.norm(np.subtract(x_rand, x_vertex)) # if dis < min_dis: # x_nearest = x_vertex # min_dis = dis # return x_nearest 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_nearest = [] for vertex in self.tree.nodes: x_vertex = self.mulp2sglp(vertex[0]) dis = np.linalg.norm(np.subtract(x_rand, x_vertex)) if dis < min_dis: q_nearest = list() q_nearest.append(vertex) min_dis = dis elif dis == min_dis: q_nearest.append(vertex) return q_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 () """ #return np.asarray([0.8,0.4]) 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, near_v, label, obs_check): """ :param: q_new: new state form: tuple (mulp, buchi) :param: near_v: near state form: tuple (mulp, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :return: extending the tree """ added = 0 cost = np.inf q_min = () for near_vertex in near_v: if q_new != near_vertex and obs_check[( q_new[0], near_vertex[0])] and self.checkTranB( near_vertex[1], self.tree.nodes[near_vertex]['label'], q_new[1]): c = self.tree.nodes[near_vertex]['cost'] + np.linalg.norm( np.subtract(self.mulp2sglp(q_new[0]), self.mulp2sglp( near_vertex[0]))) # don't consider control if c < cost: added = 1 q_min = near_vertex cost = c if added == 1: self.tree.add_node(q_new, cost=cost, label=label) self.tree.add_edge(q_min, q_new) self.add_group(q_new) if self.seg == 'pre' and q_new[1] in self.acpt: q_n = list(list(self.tree.pred[q_new].keys())[0]) cost = self.tree.nodes[tuple(q_n)]['cost'] label = self.tree.nodes[tuple(q_n)]['label'] q_n[1] = q_new[1] q_n = tuple(q_n) self.tree.add_node(q_n, cost=cost, label=label) self.tree.add_edge(q_min, q_n) self.add_group(q_n) self.goals.append(q_n) if self.seg == 'suf' and self.checkTranB(q_new[1], label, self.init[1]): print('final') if self.seg == 'suf' and self.obs_check( [self.init], q_new[0], label, 'final')[(q_new[0], self.init[0])] and self.checkTranB( q_new[1], label, self.init[1]): # if self.seg == 'suf' and self.init in near_v and obs_check[(q_new[0], self.init[0])] and self.checkTranB(q_new[1], label, self.init[1]): self.goals.append(q_new) return added def rewire(self, q_new, near_v, obs_check): """ :param: q_new: new state form: tuple (mul, buchi) :param: near_v: near state form: tuple (mul, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :return: rewiring the tree """ for near_vertex in near_v: if obs_check[(q_new[0], near_vertex[0])] and self.checkTranB( q_new[1], self.tree.nodes[q_new]['label'], near_vertex[1]): c = self.tree.nodes[q_new]['cost'] + np.linalg.norm( np.subtract(self.mulp2sglp( q_new[0]), self.mulp2sglp( near_vertex[0]))) # without considering control delta_c = self.tree.nodes[near_vertex]['cost'] - c # update the cost of node in the subtree rooted at near_vertex if delta_c > 0: # self.tree.nodes[near_vertex]['cost'] = c if not list(self.tree.pred[near_vertex].keys()): print('empty') self.tree.remove_edge( list(self.tree.pred[near_vertex].keys())[0], near_vertex) self.tree.add_edge(q_new, near_vertex) edges = dfs_labeled_edges(self.tree, source=near_vertex) for _, v, d in edges: if d == 'forward': self.tree.nodes[v][ 'cost'] = self.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) """ p_near = [] r = min( self.gamma * np.power( np.log(self.tree.number_of_nodes() + 1) / self.tree.number_of_nodes(), 1. / (self.dim * self.robot)), self.step_size) for vertex in self.tree.nodes: if np.linalg.norm(np.subtract(x_new, self.mulp2sglp( vertex[0]))) <= r: p_near.append(vertex) return p_near def obs_check(self, q_near, x_new, label, stage): """ check whether obstacle free along the line from x_near to x_new :param q_near: states in the near ball, tuple (mulp, buchi) :param x_new: new state form: multiple point :param label: label of x_new :param stage: regular stage or final stage, deciding whether it's goal state :return: dict (x_near, x_new): true (obs_free) """ # x_new = ((0.8944144022556246, 0.33267910821176216),) # label = ['l3_1'] # q_near = [(((0.8, 0.1),), 'T0_init'), (((0.9115062737314963, 0.10325925485437781),), 'T0_init')] obs_check_dict = {} for x in q_near: obs_check_dict[(x_new, x[0])] = True flag = True # indicate whether break and jump to outer loop for r in range(self.robot): for i in range(1, 11): mid = tuple( np.asarray(x[0][r]) + i / 10. * np.subtract(x_new[r], x[0][r])) mid_label = self.label(mid) if mid_label != '': mid_label = mid_label + '_' + str(r + 1) if stage == 'reg' and ( 'o' in mid_label or (mid_label != self.tree.nodes[x]['label'][r] and mid_label != label[r])): # obstacle pass through one region more than once obs_check_dict[(x_new, x[0])] = False flag = False break # elif stage == 'final' and ('o' in mid_label or (mid_label != self.tree.nodes[x]['label'][r] and mid_label != label[r] and mid_label != '')): # obstacle cannot pass through one region more than once expcet unlabeled region elif stage == 'final' and ( 'o' in mid_label or (mid_label != self.tree.nodes[x]['label'][r] and mid_label != label[r] and mid_label in self.no)): obs_check_dict[(x_new, x[0])] = False flag = False break if not flag: break return obs_check_dict def label(self, x): """ generating the label of position state :param x: position :return: label """ # whether x lies within obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if obs[1] == 'b' and np.linalg.norm(np.subtract( x, boundary[0:-1])) <= boundary[-1]: return obs[0] elif obs[1] == 'p': dictator = True for i in range(len(boundary)): if np.dot(x, boundary[i][0:-1]) + boundary[i][-1] > 0: dictator = False break if dictator == True: return obs[0] # whether x lies within regions for (regions, boundary) in iter(self.ts['region'].items()): if regions[1] == 'b' and np.linalg.norm( x - np.asarray(boundary[0:-1])) <= boundary[-1]: return regions[0] elif regions[1] == 'p': dictator = True for i in range(len(boundary)): if np.dot(x, np.asarray( boundary[i][0:-1])) + boundary[i][-1] > 0: dictator = False break if dictator == True: return regions[0] return '' def checkTranB(self, b_state, x_label, q_b_new): """ decide valid transition, whether b_state --L(x)---> q_b_new Algorithm2 in Chapter 2 Motion and Task Planning :param b_state: buchi state :param x_label: label of x :param q_b_new buchi state :return True satisfied """ b_state_succ = self.buchi_graph.succ[b_state] # q_b_new is not the successor of b_state if q_b_new not in b_state_succ: return False # b_label = self.buchi_graph.edges[(b_state, q_b_new)]['label'] # if self.t_satisfy_b(x_label, b_label): # return True truth = self.buchi_graph.edges[(b_state, q_b_new)]['truth'] if self.t_satisfy_b_truth(x_label, truth): return True def t_satisfy_b(self, x_label, b_label): """ decide whether label of self.ts_graph can satisfy label of self.buchi_graph :param x_label: label of x :param b_label: label of buchi state :return t_s_b: true if satisfied """ t_s_b = True # split label with || b_label = b_label.split('||') for label in b_label: t_s_b = True # spit label with && atomic_label = label.split('&&') for a in atomic_label: a = a.strip() a = a.strip('(') a = a.strip(')') if a == '1': continue # whether ! in an atomic proposition if '!' in a: if a[1:] in x_label: t_s_b = False break else: if not a in x_label: t_s_b = False break # either one of || holds if t_s_b: return t_s_b return t_s_b def t_satisfy_b_truth(self, x_label, truth): """ check whether transition enabled under current label :param x_label: current label :param truth: truth value making transition enabled :return: true or false """ if truth == '1': return True true_label = [ truelabel for truelabel in truth.keys() if truth[truelabel] ] for label in true_label: if label not in x_label: return False false_label = [ falselabel for falselabel in truth.keys() if not truth[falselabel] ] for label in false_label: if label in x_label: return False return True def findpath(self, goals): """ find the path backwards :param goal: goal state :return: dict path : cost """ paths = OrderedDict() for i in range(len(goals)): goal = goals[i] path = [goal] s = goal while s != self.init: s = list(self.tree.pred[s].keys())[0] if s == path[0]: print("loop") path.insert(0, s) if self.seg == 'pre': paths[i] = [self.tree.nodes[goal]['cost'], path] elif self.seg == 'suf': # path.append(self.init) paths[i] = [ self.tree.nodes[goal]['cost'] + np.linalg.norm(np.subtract(goal[0], self.init[0])), path ] return paths def mulp2sglp(self, point): """ convert multiple form point ((),(),(),...) to single form point () :param point: multiple points ((),(),(),...) :return: signle point () """ sp = [] for p in point: sp = sp + list(p) return tuple(sp) def sglp2mulp(self, point): """ convert single form point () to multiple form point ((), (), (), ...) :param point: single form point () :return: multiple form point ((), (), (), ...) """ mp = [] for i in range(self.robot): mp.append(point[i * self.dim:(i + 1) * self.dim]) return tuple(mp)
class unbiasedTree(object): """ unbiased tree for prefix and suffix parts """ def __init__(self, workspace, buchi, init_state, init_label, 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 unbiased-sampling method """ # parameters regarding workspace self.workspace = workspace.workspace self.dim = len(self.workspace) self.regions = workspace.regions self.obstacles = workspace.obs self.robot = buchi.number_of_robots # parameters regarding task self.buchi = buchi self.accept = self.buchi.buchi_graph.graph['accept'] self.init = init_state # initlizing the tree self.unbiased_tree = DiGraph(type='PBA', init=self.init) self.unbiased_tree.add_node(self.init, cost=0, label=init_label) # parameters regarding TL-RRT* algorithm self.goals = set() 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 self.gamma = (2 + 1 / 4) * np.power( (1 + 0.0 / 4) * 2.5 / (self.dim * self.robot + 1) / (1 / 4) / (1 - 0.0) * 0.84 / uni_v, 1. / (self.dim * self.robot + 1)) # 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'] # threshold for collision avoidance self.threshold = para['threshold'] def sample(self): """ sample point from the workspace :return: sampled point, tuple """ x_rand = [] for i in range(self.dim): x_rand.append(uniform(0, self.workspace[i])) return tuple(x_rand) 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 and 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.unbiased_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( map( 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, near_nodes, label, obs_check): """ 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.unbiased_tree.nodes[node]['label'], q_new[1]): c = self.unbiased_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: self.unbiased_tree.add_node(q_new, cost=cost, label=label) self.unbiased_tree.add_edge(q_min, q_new) if self.segment == 'prefix' and q_new[1] in self.accept: q_n = list(list(self.unbiased_tree.pred[q_new].keys())[0]) cost = self.unbiased_tree.nodes[tuple(q_n)]['cost'] label = self.unbiased_tree.nodes[tuple(q_n)]['label'] q_n[1] = q_new[1] q_n = tuple(q_n) if q_n != q_min: self.unbiased_tree.add_node(q_n, cost=cost, label=label) self.unbiased_tree.add_edge(q_min, q_n) self.goals.add(q_n) # if self.segment == 'suffix' and \ # self.obstacle_check([self.init], q_new[0], label)[(q_new[0], self.init[0])] \ # and self.check_transition_b(q_new[1], label, self.init[1]): # self.goals.add(q_new) elif self.segment == 'suffix' and self.init[1] == q_new[1]: self.goals.add(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.unbiased_tree.nodes[q_new]['label'], node[1]): c = self.unbiased_tree.nodes[q_new]['cost'] \ + np.linalg.norm(np.subtract(self.mulp2single(q_new[0]), self.mulp2single(node[0]))) delta_c = self.unbiased_tree.nodes[node]['cost'] - c # update the cost of node in the subtree rooted at the rewired node if delta_c > 0: self.unbiased_tree.remove_edge( list(self.unbiased_tree.pred[node].keys())[0], node) self.unbiased_tree.add_edge(q_new, node) edges = dfs_labeled_edges(self.unbiased_tree, source=node) for _, v, d in edges: if d == 'forward': self.unbiased_tree.nodes[v][ 'cost'] = self.unbiased_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.unbiased_tree.number_of_nodes() + 1) / self.unbiased_tree.number_of_nodes(), 1. / (self.dim * self.robot)), self.step_size) for node in self.unbiased_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 for (region, boundary) in iter(self.regions.items()): if LineString([Point(node[0][r]), Point(x_new[r])]).intersects(boundary) \ and region + '_' + str(r + 1) != label[r] \ and region + '_' + str(r + 1) != self.unbiased_tree.nodes[node]['label'][r]: 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 # whether x lies within regions for (region, boundary) in iter(self.regions.items()): if point.within(boundary): return region # x lies within unlabeled region 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'] if self.check_transition_b_helper(x_label, truth): return True return False def check_transition_b_helper(self, x_label, 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: if label not in x_label: 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: if label in x_label: 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() for i in range(len(goals)): goals = list(goals) goal = goals[i] path = [goal] s = goal while s != self.init: s = list(self.unbiased_tree.pred[s].keys())[0] path.insert(0, s) if self.segment == 'prefix': paths[i] = [self.unbiased_tree.nodes[goal]['cost'], path] elif self.segment == 'suffix': path.append(self.init) paths[i] = [ self.unbiased_tree.nodes[goal]['cost'] + np.linalg.norm( np.subtract(self.mulp2single(goal[0]), self.mulp2single(self.init[0]))), path ] return paths 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)
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)
class tree(object): """ construction of prefix and suffix tree """ def __init__(self, ts, buchi_graph, init, base=1e3, seg='pre'): """ :param ts: transition system :param buchi_graph: Buchi graph :param init: product initial state """ self.robot = 1 self.goals = [] self.ts = ts self.buchi_graph = buchi_graph self.init = init self.dim = len(self.ts['workspace']) self.tree = DiGraph(type='PBA', init=init) label = self.label(init[0]) if label != '': label = label + '_' + str(1) # accepting state before current node acc = set() if 'accept' in init[1]: acc.add(init) self.tree.add_node(init, cost=0, label=label, acc=acc) # already used skilles self.used = set() self.base = base self.seg = seg self.found = 10 def sample(self, num): """ sample point from the workspace :return: sampled point, tuple """ q_rand = list(self.tree.nodes())[np.random.randint( self.tree.number_of_nodes(), size=1)[0]] x_rand = [0, 0] # parallel to one axis line = np.random.randint(2, size=1)[0] x_rand[line] = q_rand[0][line] # sample another component r = round(1 / num, 10) # x_rand[1-line] = int(np.random.uniform(0, 1, size=1)[0]/r) * r + r/2 x_rand[1 - line] = round( np.random.randint(num, size=1)[0] * r + r / 2, 10) return tuple(x_rand) def acpt_check(self, q_min, q_new): """ check the accepting state in the patg leading to q_new :param q_min: :param q_new: :return: """ changed = False acc = set(self.tree.nodes[q_min]['acc']) # copy if 'accept' in q_new[1]: acc.add(q_new) # print(acc) changed = True return acc, changed def extend(self, q_new, prec_list, succ_list, label_new): """ :param: q_new: new state form: tuple (mulp, buchi) :param: near_v: near state form: tuple (mulp, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :param: succ: list of successor of the root :return: extending the tree """ added = 0 cost = np.inf q_min = () for pre in prec_list: if pre in succ_list: # do not extend if there is a corresponding root continue c = self.tree.nodes[pre]['cost'] + np.abs( q_new[0][0] - pre[0][0]) + np.abs(q_new[0][1] - pre[0][1]) if c < cost: added = 1 q_min = pre cost = c if added == 1: self.tree.add_node(q_new, cost=cost, label=label_new) self.tree.nodes[q_new]['acc'] = set( self.acpt_check(q_min, q_new)[0]) self.tree.add_edge(q_min, q_new) return added def rewire(self, q_new, succ_list): """ :param: q_new: new state form: tuple (mul, buchi) :param: near_v: near state form: tuple (mul, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :return: rewiring the tree """ for suc in succ_list: # root if suc == self.init: continue c = self.tree.nodes[q_new]['cost'] + np.abs( q_new[0][0] - suc[0][0]) + np.abs(q_new[0][1] - suc[0][1]) delta_c = self.tree.nodes[suc]['cost'] - c # update the cost of node in the subtree rooted at near_vertex if delta_c > 0: self.tree.remove_edge(list(self.tree.pred[suc].keys())[0], suc) self.tree.add_edge(q_new, suc) edges = dfs_labeled_edges(self.tree, source=suc) acc, changed = self.acpt_check(q_new, suc) self.tree.nodes[suc]['acc'] = set(acc) for u, v, d in edges: if d == 'forward': self.tree.nodes[v][ 'cost'] = self.tree.nodes[v]['cost'] - delta_c if changed: self.tree.nodes[v]['acc'] = set( self.acpt_check(u, v)[0]) # copy # better to research the goal but abandon the implementation def prec(self, q_new, label_new, obs_check): """ find the predcessor of q_new :param q_new: new product state :return: label_new: label of new """ p_prec = [] for vertex in self.tree.nodes: if q_new != vertex and self.obs_check(vertex, q_new[0], label_new, obs_check) \ and self.checkTranB(vertex[1], self.tree.nodes[vertex]['label'], q_new[1]): p_prec.append(vertex) return p_prec def succ(self, q_new, label_new, obs_check): """ find the successor of q_new :param q_new: new product state :return: label_new: label of new """ p_succ = [] for vertex in self.tree.nodes: if q_new != vertex and self.obs_check(vertex, q_new[0], label_new, obs_check) \ and self.checkTranB(q_new[1], self.tree.nodes[q_new]['label'], vertex[1]): p_succ.append(vertex) return p_succ def obs_check(self, q_tree, x_new, label_new, obs_check): """ check whether obstacle free along the line from q_tree to x_new :param q_tree: vertex in the tree :param x_new: :param label_new: :return: true or false """ if q_tree[0][0] != x_new[0] and q_tree[0][1] != x_new[1]: return False if (q_tree[0], x_new) in obs_check.keys(): return obs_check[(q_tree[0], x_new)] if (x_new, q_tree[0]) in obs_check.keys(): return obs_check[(x_new, q_tree[0])] # the line connecting two points crosses an obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if LineString([Point(q_tree[0]), Point(x_new)]).intersects(boundary): obs_check[(q_tree[0], x_new)] = False obs_check[(x_new, q_tree[0])] = False return False for (region, boundary) in iter(self.ts['region'].items()): if LineString([Point(q_tree[0]), Point(x_new)]).intersects(boundary) \ and region + '_' + str(1) != label_new \ and region + '_' + str(1) != self.tree.nodes[q_tree]['label']: obs_check[(q_tree[0], x_new)] = False obs_check[(x_new, q_tree[0])] = False return False obs_check[(q_tree[0], x_new)] = True obs_check[(x_new, q_tree[0])] = True return True def label(self, x): """ generating the label of position state :param x: position :return: label """ point = Point(x) # whether x lies within obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if point.within(boundary): return obs # whether x lies within regions for (region, boundary) in iter(self.ts['region'].items()): if point.within(boundary): return region # x lies within unlabeled region return '' def checkTranB(self, b_state, x_label, q_b_new): """ decide valid transition, whether b_state --L(x)---> q_b_new :param b_state: buchi state :param x_label: label of x :param q_b_new buchi state :return True satisfied """ b_state_succ = self.buchi_graph.succ[b_state] # q_b_new is not the successor of b_state if q_b_new not in b_state_succ: return False truth = self.buchi_graph.edges[(b_state, q_b_new)]['truth'] if self.t_satisfy_b_truth(x_label, truth): return True return False def t_satisfy_b_truth(self, x_label, truth): """ check whether transition enabled under current label :param x_label: current label :param truth: truth value making transition enabled :return: true or false """ if truth == '1': return True true_label = [ truelabel for truelabel in truth.keys() if truth[truelabel] ] for label in true_label: if label not in x_label: return False false_label = [ falselabel for falselabel in truth.keys() if not truth[falselabel] ] for label in false_label: if label in x_label: return False return True def findpath(self, goals): """ find the path backwards :param goals: goal state :return: dict path : cost """ paths = OrderedDict() for i in range(len(goals)): goal = goals[i] path = [goal] s = goal while s != self.init: s = list(self.tree.pred[s].keys())[0] path.insert(0, s) paths[i] = [self.tree.nodes[goal]['cost'], path] return paths
def tree0(weight_value, startwindow, term): print 'start window:', startwindow # windowGraph = {} cliqueGraph = DiGraph() dic_term = {} dic_last_time = {} dic_temp = {} dic_term_num = {} dic_intersect_level = {} # term = 183 root = 0 cliqueGraph.add_node(root, annotation='root', windowsize='root', weight_value='root') w = data.shape[1] i = 0 q = 0 for window in range(startwindow, w): dic_intersect_level.clear() #print window ## mine if window == startwindow: for clique in find_cliques(windowGraph[window]): if len(clique) >size_clique: cliqueGraph.add_node(term, annotation=list(clique), windowsize=[window], weight=weight_value) # generate a term cliqueGraph.add_edge(root, term) dic_term[frozenset(clique)] = [window] # dic_term 记录 window和clique or Dic_term records window and clique dic_term_num[frozenset(clique)] = term # dic_term_num 记录 term 序号和clique or Dic_term_num record term number and clique dic_last_time[frozenset(clique)] = [window] # dic_last_time 记录上一时刻生成的交集 用于下一时刻的比较 or Dic_last_time records the intersection generated at the last moment for comparison at the next moment term = term + 1 print 'for start window ' else: continue # print len(dic_last_time), len(dic_term), cliqueGraph.number_of_nodes() else: for clique in find_cliques(windowGraph[window]): if len(clique) > size_clique: #print window, 'clique:', clique ## mine for key, value in dic_last_time.items(): # key 是clique ,value是 [window] or Key is clique, value is [window] intersect = sorted(set(key).intersection(set(clique))) q = 0 # if len(intersect) >= size_clique: if len(intersect) >= size_clique: #print 'intersect', intersect # 同一层判断交集之间是否有重复的父子关系。 每生成一个交集, 判断当前层的其他term和交集的关系。or The same layer determines whether there are #duplicate parent-child relationships between intersections. Each generation of an intersection determines the relationship #between other terms and intersections of the current layer. for ik, iv in dic_intersect_level.items(): if set(intersect) == (set(ik)): # 生成一模一样的交集 or Generate exactly the same intersection # 判断两个的编号是否一样?or Is the two numbers the same? if dic_term_num[frozenset(key)] != dic_term_num[frozenset(ik)]: cliqueGraph.add_edge(dic_term_num[frozenset(key)], dic_term_num[frozenset(ik)]) q = 1 break elif set(intersect).issuperset(set(ik)): # 生成了超集 or Superset generated cliqueGraph.remove_node(dic_term_num[frozenset(ik)]) dic_term.pop(frozenset(ik)) # 从四个字典中都删除该节点的信息 or Delete the node's information from all four dictionaries dic_term_num.pop(frozenset(ik)) dic_intersect_level.pop(frozenset(ik)) dic_temp.pop(frozenset(ik)) elif set(intersect).issubset(set(ik)): # 生成了子集 or Generated subset q = 1 break if q == 1: continue dic_intersect_level[frozenset(intersect)] = 1 if dic_term.has_key(frozenset(intersect)): # 交集已经出现过 or Intersection has appeared parent = cliqueGraph.predecessors(dic_term_num[frozenset(intersect)]) children = cliqueGraph.successors(dic_term_num[frozenset(intersect)]) #print 'parent',len(parent) if len(parent) > 0: # 是交集生成的term,则重定向 or Is the intersection of generated term, then redirect cliqueGraph.add_node(term, annotation=list(intersect), windowsize=value + [window], weight=weight_value) for p in parent: cliqueGraph.add_edge(p, term) # 连边 // Edge for c in children: cliqueGraph.add_edge(term, c) # 连边 // edge cliqueGraph.remove_node(dic_term_num[frozenset(intersect)]) # 从图中删除冗余结点 or Remove redundant nodes from the figure # print 'deleted intersect nodes:',dic_term_num[frozenset(intersect)] i = i + 1 dic_term.pop(frozenset(intersect)) # 字典中删除 // Delete in dictionary dic_term_num.pop(frozenset(intersect)) dic_term[frozenset(intersect)] = value + [window] # 新节点插入字典 // New node insert dictionary dic_term_num[frozenset(intersect)] = term dic_temp[frozenset(intersect)] = value + [window] # 记录到dic_temp里 // Record to dic_temp term = term + 1 continue else: # 是window生成的term // Is the term generated by the window continue else: # 交集没有出现过, 则生成新的term // No intersection occurs, then a new term is generated # print 'new term intersect never appear:', term cliqueGraph.add_node(term, annotation=list(intersect), windowsize=value + [window], weight=weight_value) # generate a term cliqueGraph.add_edge(dic_term_num[frozenset(key)], term) # 连边,变化:只连接交集作为父亲。// Edge, change: Only connect intersections as fathers. dic_term[frozenset(intersect)] = value + [window] # 新节点插入字典 // New node insert dictionary dic_term_num[frozenset(intersect)] = term dic_temp[frozenset(intersect)] = value + [window] # 记录到dic_temp里 // Record to dic_temp term = term + 1 else: continue else: continue dic_last_time.clear() for key, value in dic_temp.items(): dic_last_time[key] = value dic_temp.clear() print 'window', startwindow, 'size is', cliqueGraph.number_of_nodes(), cliqueGraph.number_of_edges()## mine # print 'deleted nodes:', i # fw = open('0904edges_remove.txt', 'w') # fw2 = open('0904terms_remove.txt', 'w') # fw.write('parent' + '\t' + 'child' + '\n') # for edge in cliqueGraph.edges(): # fw.write(str(edge[0]) + '\t' + str(edge[1]) + '\n') # fw.close() # fw2.write('term_id' + '\t' + 'anno_genes' + '\t' + 'window' + '\t' + 'gene_size' + '\t' + 'window_size' + '\n') # for key, value in dic_term.items(): # fw2.write(str(dic_term_num[key]) + '\t' + str(key) + '\t' + str(value) + '\t' + str(len(key)) + '\t' + str(len(value)) + '\n') # fw2.close() # for nodes in cliqueGraph.nodes(): # if cliqueGraph.degree(nodes) == 0: # print nodes return cliqueGraph, dic_term, dic_term_num, term
class BiasedTree(object): """ biased tree for prefix and suffix parts """ def __init__(self, workspace, buchi, init_state, init_label, 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 = workspace.workspace self.dim = len(self.workspace) self.regions = workspace.regions self.obstacles = workspace.obs self.robot = buchi.number_of_robots # parameters regarding task self.buchi = buchi 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) # 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'] # 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, closest node q_p_closest in terms of transitions """ # 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']): 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'] x_rand = list(q_p_closest[0]) return self.buchi_guided_sample_by_truthvalue( truth, x_rand, q_p_closest, self.biased_tree.nodes[q_p_closest]['label']) def buchi_guided_sample_by_truthvalue(self, truth, x_rand, q_p_closest, x_label): """ 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 """ if truth == '1': return q_p_closest[0], q_p_closest else: for key in truth: # move towards the target position if truth[key] and key not in x_label: pair = key.split('_') # region-robot pair robot_index = int(pair[1]) - 1 orig_x_rand = x_rand[ robot_index] # save for further recover while True: x_rand[robot_index] = orig_x_rand # recover if np.random.uniform(0, 1, 1) <= self.y_rand: target = self.get_target(orig_x_rand, pair[0]) x_rand[ robot_index] = self.gaussian_guided_towards_target( orig_x_rand, target) else: x_rand_i = [ uniform(0, self.workspace[i]) for i in range(self.dim) ] x_rand[robot_index] = tuple(x_rand_i) # 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 return tuple(x_rand), q_p_closest 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): """ 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 """ tg = self.regions[target].centroid.coords[0] shortest = self.g.shortest_path(vg.Point(init[0], init[1]), vg.Point(tg[0], tg[1])) 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 = self.get_truncated_normal(0, 1 / 3 * self.workspace[0], 0, np.inf).rvs() # d = self.get_truncated_normal(np.linalg.norm(np.subtract(x, target)), 1/3/3, 0, np.inf) 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 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 and 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, near_nodes, label, obs_check): """ add the new state 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 = () STL_cost = [] i = 0 # loop over all nodes in near_nodes print("near_nodes", near_nodes, "\n") 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]): STL_cost = calculate_STL_cost(self.mulp2single(q_new[0]), self.mulp2single(node[0])) c = self.biased_tree.nodes[node]['cost'] \ + np.linalg.norm(np.subtract(self.mulp2single(q_new[0]), self.mulp2single(node[0])))*STL_cost if c < cost: print("\n Alter cost in extend for node:", node, "\n") print('STL_cost_extend save:', STL_cost, '\n') print("new cost:", c, "\n") print("previous cost:", cost, "\n") added = True q_min = node cost = c i += 1 if added: self.biased_tree.add_node(q_new, cost=cost, label=label) 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'] print("cost prefix", cost, "\n") 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) 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 """ STL_cost = [] i = 0 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]): STL_cost = calculate_STL_cost(self.mulp2single(q_new[0]), self.mulp2single(node[0])) c = self.biased_tree.nodes[q_new]['cost'] \ + np.linalg.norm(np.subtract(self.mulp2single(q_new[0]), self.mulp2single(node[0])))*STL_cost 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: print("node", node, "\n") print('STL_cost_rewire save:', STL_cost, '\n') print("final cost c:", c, "\n") print("previous cost:", self.biased_tree.nodes[node]['cost'], "\n") 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 i += 1 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 for (region, boundary) in iter(self.regions.items()): if LineString([Point(node[0][r]), Point(x_new[r])]).intersects(boundary) \ and region + '_' + str(r + 1) != label[r] \ and region + '_' + str(r + 1) != self.biased_tree.nodes[node]['label'][r]: 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 # whether x lies within regions for (region, boundary) in iter(self.regions.items()): if point.within(boundary): return region # x lies within unlabeled region 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'] if self.check_transition_b_helper(x_label, truth): return True return False def check_transition_b_helper(self, x_label, 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: if label not in x_label: 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: if label in x_label: 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() for i in range(len(goals)): goal = goals[i] path = [goal] s = goal while s != self.init: s = list(self.biased_tree.pred[s].keys())[0] path.insert(0, s) paths[i] = [self.biased_tree.nodes[goal]['cost'], path] return paths 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)
class second_tree(object): """ construction of prefix and suffix tree """ def __init__(self, n_robot, acpt, ts, buchi_graph, init, step_size, no): """ :param acpt: accepting state :param ts: transition system :param buchi_graph: Buchi graph :param init: product initial state """ self.robot = n_robot self.acpt = acpt self.goals = [] self.ts = ts self.buchi_graph = buchi_graph self.init = init self.step_size = step_size self.dim = len(self.ts['workspace']) 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 self.tree = DiGraph(type='PBA', init=init) self.group = dict() # label of init label = [] for i in range(self.robot): l = self.label(self.init[0][i]) # exists one sampled point lies within obstacles if l != '': l = l + '_' + str(i + 1) label.append(l) self.tree.add_node(self.init, cost=0, label=label) # label of acpt label = [] for i in range(self.robot): l = self.label(self.init[0][i]) # exists one sampled point lies within obstacles if l != '': l = l + '_' + str(i + 1) label.append(l) self.tree.add_node(self.acpt, cost=0, label=label) # index of robot whose location needs to be changed self.change = self.match(label) # best node self.x_min = self.init self.c = np.linalg.norm(np.subtract(self.init[0], self.acpt[0])) # probability for selecting a node self.p = 0.9 # target point self.weight = 0.5 # threshold for collision avoidance self.threshold = 0.005 # polygon obstacle polys = [[ vg.Point(0.4, 1.0), vg.Point(0.4, 0.7), vg.Point(0.6, 0.7), vg.Point(0.6, 1.0) ], [ vg.Point(0.3, 0.2), vg.Point(0.3, 0.0), vg.Point(0.7, 0.0), vg.Point(0.7, 0.2) ]] self.g = vg.VisGraph() self.g.build(polys, status=False) # region that has ! preceding it self.no = no # biased sampling def match(self, label): """ match between self.init and self.acpt :param label: label of acpt :return: """ x = self.init x_new = self.acpt change = [] for r in range(self.robot): flag = True # the line connecting two points crosses an obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if LineString([Point(x[0][r]), Point(x_new[0][r])]).intersects(boundary): change.append(r) flag = False break if not flag: continue for (region, boundary) in iter(self.ts['region'].items()): if LineString([Point(x[0][r]), Point(x_new[0][r])]).intersects(boundary) \ and region + '_' + str(r + 1) != label[r] \ and region + '_' + str(r + 1) != self.tree.nodes[x]['label'][r]: change.append(r) return change def get_truncated_normal(self, mean=0, sd=1, low=0, upp=10): return truncnorm((low - mean) / sd, (upp - mean) / sd, loc=mean, scale=sd) def trunc(self, value): if value < 0: return 0 elif value > 1: return 1 else: return value def collision_avoidance(self, x, index): """ check whether any robots are collision-free from index-th robot :param x: all robots :param index: index-th robot :return: true collision free """ for i in range(len(x)): # if i != index and np.linalg.norm(np.subtract(x[i], x[index])) <= self.threshold: if i != index and np.fabs( x[i][0] - x[index][0]) <= self.threshold and np.fabs( x[i][1] - x[index][1]) <= self.threshold: return False return True def target(self, init, tg): """ find the closest vertex in the short path from init to target :param init: inital point :param target: target labeled region :param regions: regions :return: closest vertex """ shortest = self.g.shortest_path(vg.Point(init[0], init[1]), vg.Point(tg[0], tg[1])) return shortest[1].x, shortest[1].y def gaussian_guided(self, x, target): """ calculate new point following gaussian dist guided by the target :param x: mean point :param target: target point :return: new point """ # d = np.linalg.norm(np.subtract(x, target)) # angle = np.arctan2(target[1] - x[1], target[0] - x[0]) d = self.get_truncated_normal(0, 1 / 3, 0, np.inf) d = d.rvs() 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(x) for x in x_rand] return tuple(x_rand) def guided_sample_by_destination(self, x_rand): """ sample guided by truth value :param truth: the value making transition occur :param x_rand: random selected node :param x_label: label of x_rand :param regions: regions :return: new sampled point """ x = list(x_rand) for ind in self.change: # same component with self.acpt if x[ind] == self.acpt[0][ind]: continue orig_x_rand = x_rand[ind] # save while 1: x[ind] = orig_x_rand # recover if np.random.uniform(0, 1, 1) <= self.weight: tg = self.target(orig_x_rand, self.acpt[0][ind]) x[ind] = self.gaussian_guided(orig_x_rand, tg) else: xi_rand = [] for i in range(self.dim): xi_rand.append(uniform(0, self.ts['workspace'][i])) x[ind] = tuple(xi_rand) if self.collision_avoidance(x, ind): break # x_rand return self.mulp2sglp(x) def sample(self): """ sample point from the workspace :return: sampled point, tuple """ while True: # random select node as q_rand if np.random.uniform(0, 1, 1) <= self.p: q_rand = self.x_min else: q_rand = choice(list(self.tree.nodes)) # since self.acpt is in the tree, maybe it's rootless if q_rand == self.acpt: continue else: break return self.guided_sample_by_destination(q_rand[0]), q_rand 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, near_v, label, obs_check): """ :param: q_new: new state form: tuple (mulp, buchi) :param: near_v: near state form: tuple (mulp, buchi) :param: obs_check: check obstacle free form: dict { (mulp, mulp): True } :return: extending the tree """ added = 0 cost = np.inf q_min = () for near_vertex in near_v: if q_new != near_vertex and obs_check[( q_new[0], near_vertex[0])] and self.checkTranB( near_vertex[1], self.tree.nodes[near_vertex]['label'], q_new[1]): c = self.tree.nodes[near_vertex]['cost'] + np.linalg.norm( np.subtract(self.mulp2sglp(q_new[0]), self.mulp2sglp( near_vertex[0]))) # don't consider control if c < cost: added = 1 q_min = near_vertex cost = c if added == 1: self.tree.add_node(q_new, cost=cost, label=label) self.tree.add_edge(q_min, q_new) # update the distance from acpt if np.linalg.norm(np.subtract(q_new[0], self.acpt[0])) < self.c: self.x_min = q_new self.c = np.linalg.norm(np.subtract(q_new[0], self.acpt[0])) if self.obs_check([self.acpt], q_new[0], label, 'final')[(q_new[0], self.acpt[0])]: self.goals.append(q_new) return added 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) """ p_near = [] r = min( self.gamma * np.power( np.log(self.tree.number_of_nodes() + 1) / self.tree.number_of_nodes(), 1. / (self.dim * self.robot)), self.step_size) for vertex in self.tree.nodes: if np.linalg.norm(np.subtract(x_new, self.mulp2sglp( vertex[0]))) <= r: p_near.append(vertex) # if len(p_near) > 0: # break return p_near def obs_check(self, q_near, x_new, label, stage): """ check whether obstacle free along the line from x_near to x_new :param q_near: states in the near ball, tuple (mulp, buchi) :param x_new: new state form: multiple point :param label: label of x_new :param stage: regular stage or final stage, deciding whether it's goal state :return: dict (x_near, x_new): true (obs_free) """ obs_check_dict = {} checked = set() for x in q_near: if x[0] in checked: continue checked.add(x[0]) obs_check_dict[(x_new, x[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.ts['obs'].items()): if LineString([Point(x[0][r]), Point(x_new[r])]).intersects(boundary): obs_check_dict[(x_new, x[0])] = False flag = False break if not flag: break for (region, boundary) in iter(self.ts['region'].items()): if LineString([Point(x[0][r]), Point(x_new[r])]).intersects(boundary) \ and region + '_' + str(r + 1) != label[r] \ and region + '_' + str(r + 1) != self.tree.nodes[x]['label'][r]: if stage == 'reg' or (stage == 'final' and region in self.no): obs_check_dict[(x_new, x[0])] = False flag = False break if not flag: break return obs_check_dict def label(self, x): """ generating the label of position state :param x: position :return: label """ point = Point(x) # whether x lies within obstacle for (obs, boundary) in iter(self.ts['obs'].items()): if point.within(boundary): return obs # whether x lies within regions for (region, boundary) in iter(self.ts['region'].items()): if point.within(boundary): return region # x lies within unlabeled region return '' def checkTranB(self, b_state, x_label, q_b_new): """ decide valid transition, whether b_state --L(x)---> q_b_new Algorithm2 in Chapter 2 Motion and Task Planning :param b_state: buchi state :param x_label: label of x :param q_b_new buchi state :return True satisfied """ b_state_succ = self.buchi_graph.succ[b_state] # q_b_new is not the successor of b_state if q_b_new not in b_state_succ: return False # b_label = self.buchi_graph.edges[(b_state, q_b_new)]['label'] # if self.t_satisfy_b(x_label, b_label): # return True truth = self.buchi_graph.edges[(b_state, q_b_new)]['truth'] if self.t_satisfy_b_truth(x_label, truth): return True def t_satisfy_b(self, x_label, b_label): """ decide whether label of self.ts_graph can satisfy label of self.buchi_graph :param x_label: label of x :param b_label: label of buchi state :return t_s_b: true if satisfied """ t_s_b = True # split label with || b_label = b_label.split('||') for label in b_label: t_s_b = True # spit label with && atomic_label = label.split('&&') for a in atomic_label: a = a.strip() a = a.strip('(') a = a.strip(')') if a == '1': continue # whether ! in an atomic proposition if '!' in a: if a[1:] in x_label: t_s_b = False break else: if not a in x_label: t_s_b = False break # either one of || holds if t_s_b: return t_s_b return t_s_b def t_satisfy_b_truth(self, x_label, truth): """ check whether transition enabled under current label :param x_label: current label :param truth: truth value making transition enabled :return: true or false """ if truth == '1': return True true_label = [ truelabel for truelabel in truth.keys() if truth[truelabel] ] for label in true_label: if label not in x_label: return False false_label = [ falselabel for falselabel in truth.keys() if not truth[falselabel] ] for label in false_label: if label in x_label: return False return True def findpath(self, goals): """ find the path backwards :param goal: goal state :return: dict path : cost """ paths = OrderedDict() for i in range(len(goals)): goal = goals[i] path = [goal] s = goal while s != self.init: s = list(self.tree.pred[s].keys())[0] if s == path[0]: print("loop") path.insert(0, s) paths[i] = [ self.tree.nodes[goal]['cost'] + np.linalg.norm(np.subtract(goal[0], self.init[0])), path ] return paths def mulp2sglp(self, point): """ convert multiple form point ((),(),(),...) to single form point () :param point: multiple points ((),(),(),...) :return: signle point () """ sp = [] for p in point: sp = sp + list(p) return tuple(sp) def sglp2mulp(self, point): """ convert single form point () to multiple form point ((), (), (), ...) :param point: single form point () :return: multiple form point ((), (), (), ...) """ mp = [] for i in range(self.robot): mp.append(point[i * self.dim:(i + 1) * self.dim]) return tuple(mp)