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 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
def to_directed_tree_recur(di_g: DiGraph, node: int) -> None:
children: AtlasView = di_g[node]
if len(children) != 0:
for child in children:
di_g.remove_edge(child, node)
to_directed_tree_recur(di_g, child)
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 buchi_graph(object):
""" construct buchi automaton graph
Parameter:
formula: LTL formula specifying task
"""
def __init__(self, formula, formula_comp, exclusion):
self.formula = formula
self.formula_comp = formula_comp
self.exclusion = exclusion
def formulaParser(self):
"""replace letter with symbol
"""
indicator = 'FG'
if [True for i in indicator if i in self.formula]:
self.formula.replace('F', '<>').replace('G', '[]')
def execLtl2ba(self):
""" given formula, exectute the ltl2ba
Parameter:
buchi_str: output string of program ltl2ba (utf-8 format)
"""
dirname = os.path.dirname(__file__)
self.buchi_str = subprocess.check_output(dirname + "/./ltl2ba -f \"" + self.formula + "\"", shell=True).decode("utf-8")
def buchiGraph(self):
"""parse the output of ltl2ba
Parameter:
buchi_graph: Graph of buchi automaton
"""
# find all states
state_re = re.compile(r'\n(\w+):\n\t')
state_group = re.findall(state_re, self.buchi_str)
# find initial and accepting states
init = [s for s in state_group if 'init' in s]
accep = [s for s in state_group if 'accept' in s]
"""
Format:
buchi_graph.node = NodeView(('T0_init', 'T1_S1', 'accept_S1'))
buchi_graph.edges = OutEdgeView([('T0_init', 'T0_init'), ('T0_init', 'T1_S1'),....])
buchi_graph.succ = AdjacencyView({'T0_init': {'T0_init': {'label': '1'}, 'T1_S1': {'label': 'r3'}}})
"""
self.buchi_graph = DiGraph(type='buchi', init=init, accept=accep)
order_key = list(self.formula_comp.keys())
order_key.sort(reverse=True)
for state in state_group:
# for each state, find transition relation
# add node
self.buchi_graph.add_node(state)
state_if_fi = re.findall(state + r':\n\tif(.*?)fi', self.buchi_str, re.DOTALL)
if state_if_fi:
relation_group = re.findall(r':: (\(.*?\)) -> goto (\w+)\n\t', state_if_fi[0])
for (labell, state_dest) in relation_group:
# whether the edge is feasible in terms of unit atomic proposition
label = self.InitialDelInfesEdge(labell)
if not label or label.isspace():
continue
# add edge
for k in order_key:
if k >= 10:
label = label.replace('e_{0}'.format(k), self.formula_comp[k])
else:
label = label.replace('e{0}'.format(k), self.formula_comp[k])
# if '!' in label:
# label = self.PutNotInside(label)
self.buchi_graph.add_edge(state, state_dest, label=label)
return self.buchi_graph
def ShorestPathBtRg(self, regions):
"""
calculate shoresr path between any two labeled regions
:param regions: regions
:return: dict (region, region) : length
"""
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)]]
g = vg.VisGraph()
g.build(polys, status=False)
min_len_region = dict()
for key1, value1 in regions.items():
for key2, value2 in regions.items():
init = value1[:2]
tg = value2[:2]
# shorest path between init and tg point
shortest = g.shortest_path(vg.Point(init[0], init[1]), vg.Point(tg[0], tg[1]))
# (key2, key1) is already checked
if (key2, key1) in min_len_region.keys():
min_len_region[(key1, key2)] = min_len_region[(key2, key1)]
else:
# different regions
if key1 != key2:
dis = 0
for i in range(len(shortest)-1):
dis = dis + np.linalg.norm(np.subtract((shortest[i].x, shortest[i].y), (shortest[i+1].x, shortest[i+1].y)))
min_len_region[(key1, key2)] = dis
# same region
else:
min_len_region[(key1, key2)] = 0
return min_len_region
def RobotRegion(self, exp, robot):
"""
pair of robot and corresponding regions in the expression
:param exp: logical expression
:param robot: # of robots
:return: dic of robot index : regions
exp = 'l1_1 & l3_1 & l4_1 & l4_6 | l3_4 & l5_6'
{1: ['l1_1', 'l3_1', 'l4_1'], 4: ['l3_4'], 6: ['l4_6', 'l5_6']}
"""
robot_region_dict = dict()
for r in range(robot):
findall = re.findall(r'(l\d+?_{0})[^0-9]'.format(r + 1), exp)
if findall:
robot_region_dict[str(r + 1)] = findall
return robot_region_dict
def FeasTruthTable(self, exp, robot_region):
"""
Find feasible truth table to make exp true
:param exp: expression
:return:
"""
if exp == '(1)':
return '1'
sgl_value = []
for key, value in robot_region.items():
if len(value) == 1:
sgl_value.append(value[0])
# set all to be false
exp1 = to_cnf(exp)
value_in_exp = [value.name for value in exp1.atoms()]
subs = {true_rb_rg: False for true_rb_rg in value_in_exp}
if exp1.subs(subs):
return subs
# set one to be true, the other to be false
for prod in itertools.product(*robot_region.values()):
exp1 = exp
# set one specific item to be true
for true_rb_rg in prod:
# set the other to be false
value_cp = list(robot_region[true_rb_rg.split('_')[1]])
if len(value_cp) > 1:
value_cp.remove(true_rb_rg)
# replace the rest with same robot to be ~
for v_remove in value_cp:
exp1 = exp1.replace(v_remove, '~' + true_rb_rg)
# simplify
exp1 = to_cnf(exp1)
# all value in expression
value_in_exp = [value.name for value in exp1.atoms()]
# all single value in expression
sgl_value_in_exp = [value for value in value_in_exp if value in sgl_value]
# not signle value in expression
not_sgl_value_in_exp = [value for value in value_in_exp if value not in sgl_value]
subs1 = {true_rb_rg: True for true_rb_rg in not_sgl_value_in_exp}
tf = [False, True]
# if type(exp1) == Or:
# tf = [False, True]
if len(sgl_value_in_exp):
for p in itertools.product(*[tf] * len(sgl_value_in_exp)):
subs2 = {sgl_value_in_exp[i]: p[i] for i in range(len(sgl_value_in_exp))}
subs = {**subs1, **subs2}
if exp1.subs(subs):
return subs
else:
if exp1.subs(subs1):
return subs1
return []
def DelInfesEdge(self, robot):
"""
Delete infeasible edge
:param buchi_graph: buchi automaton
:param robot: # robot
"""
TobeDel = []
# print(self.buchi_graph.number_of_edges())
i = 0
for edge in self.buchi_graph.edges():
i = i+1
# print(i)
b_label = self.buchi_graph.edges[edge]['label']
# multiple labels
if ') && (' in b_label:
TobeDel.append(edge)
continue
if b_label != '(1)':
exp = b_label.replace('||', '|').replace('&&', '&').replace('!', '~')
truth = satisfiable(exp, algorithm="dpll")
truth_table = dict()
for key, value in truth.items():
truth_table[key.name] = value
if not truth_table:
TobeDel.append(edge)
else:
self.buchi_graph.edges[edge]['truth'] = truth_table
else:
self.buchi_graph.edges[edge]['truth'] = '1'
for edge in TobeDel:
self.buchi_graph.remove_edge(edge[0], edge[1])
# print(self.buchi_graph.number_of_edges())
def InitialDelInfesEdge(self, orig_label):
div_by_or = orig_label.split(') || (')
for item in div_by_or:
feas = True
for excl in self.exclusion:
# mutual exclusion term exist
if excl[0] in item and excl[1] in item and '!{0}'.format(excl[0]) not in item and '!{0}'.format(excl[1]) not in item:
feas = False
break
if not feas:
item = item.strip('(').strip(')')
item = '(' + item + ')'
orig_label = orig_label.replace(' '+item+' ||', '').replace(item+' || ','').replace(' || '+item,'').replace(item,'')
return orig_label
def MinLen(self):
"""
search the shorest path from a node to another, weight = 1, i.e. # of state in the path
:param buchi_graph:
:return: dict of pairs of node : length of path
"""
min_qb_dict = dict()
for node1 in self.buchi_graph.nodes():
for node2 in self.buchi_graph.nodes():
if node1 != node2 and 'accept' in node2:
try:
l, _ = nx.algorithms.single_source_dijkstra(self.buchi_graph, source=node1, target=node2)
except nx.exception.NetworkXNoPath:
l = np.inf
min_qb_dict[(node1, node2)] = l
elif node1 == node2 and 'accept' in node2:
l = np.inf
for succ in self.buchi_graph.succ[node1]:
try:
l0, _ = nx.algorithms.single_source_dijkstra(self.buchi_graph, source=succ, target=node1)
except nx.exception.NetworkXNoPath:
l0 = np.inf
if l0 < l:
l = l0 + 1
min_qb_dict[(node1, node2)] = l
return min_qb_dict
# def MinLen_Cost(self):
# """
# search the shorest path from a node to another, weight = cost
# :param buchi_graph:
# :return: dict of pairs of node : length of path
# """
# min_qb_dict = dict()
# for node1 in self.buchi_graph.nodes():
# for node2 in self.buchi_graph.nodes():
# c = np.inf
# if node1 != node2:
# try:
# path = nx.all_simple_paths(self.buchi_graph, source=node1, target=node2)
# for i in range(len(path)-2):
# word_init = self.buchi_graph.edges[(path[i], path[i+1])]['label']
# word_tg = self.buchi_graph.edges[(path[i+1], path[i+2])]['label']
# # calculate distance travelled from word_init to word_tg
# t_s_b = True
# # split label with ||
# label_init = word_init.split('||')
# label_tg = word_tg.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
# except nx.exception.NetworkXNoPath:
# c = np.inf
# else:
# c = 0
# min_qb_dict[(node1, node2)] = c
#
# return min_qb_dict
def FeasAcpt(self, min_qb):
"""
delte infeasible final state
:param buchi_graph: buchi automaton
:param min_qb: dict of pairs of node : length of path
"""
accept = self.buchi_graph.graph['accept']
for acpt in accept:
if min_qb[(self.buchi_graph.graph['init'][0], acpt)] == np.inf or min_qb[(acpt, acpt)] == np.inf:
self.buchi_graph.graph['accept'].remove(acpt)
def PutNotInside(self, str):
"""
put not inside the parenthesis !(p1 && p2) -> !p1 or !p2
:param str: old
:return: new
"""
substr = re.findall("(!\(.*?\))", str) # ['!(p1 && p2)', '!(p4 && p5)']
for s in substr:
oldstr = s.strip().strip('!').strip('(').strip(')')
nstr = ''
for ss in oldstr.split():
if '&&' in ss:
nstr = nstr + ' or '
elif 'or' in ss:
nstr = nstr + ' && '
else:
nstr = nstr + '!' + ss
str = str.replace(s, nstr)
return str
def label2sat(self):
for edge in self.buchi_graph.edges():
label = self.buchi_graph.edges[edge]['label']
label = label.replace('||', '|').replace('&&', '&').replace('!', '~')
exp1 = to_cnf(label)
self.buchi_graph.edges[edge]['label'] = exp1
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
class tree(object):
""" construction of prefix and suffix tree
"""
def __init__(self, ts, buchi_graph, init, final, seg, env):
"""
: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)
self.tree.add_node(init, cost=0, label=label)
self.group = dict()
self.add_group(init)
self.b_final = final
self.seg = seg
self.p = 0.9
self.env = env
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 sample(self, buchi_graph, min_qb_dict, num_grid, centers):
"""
sample point from the workspace
:return: sampled point, tuple
"""
# collects the buchi state in the tree with minimum distance to the final state
b_min = self.min2final(min_qb_dict, self.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)
q_rand = []
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 = []
label = self.label(q_rand[0])
if label != '':
label = label + '_' + str(1)
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(
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, self.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, self.b_final)] - 1 == min_qb_dict[(
succ, self.b_final)] or succ == self.b_final:
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]))]
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, num_grid,
centers)
def buchi_guided_sample_by_truthvalue(self, truth, x_rand, num_grid,
centers):
"""
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
"""
# stay
if truth == '1':
return x_rand
p_new = 0.8
region = ''
for key in truth:
if truth[key]:
region = key
if not region:
return x_rand
# select second point withi high probability
if uniform(0, 1, 1) < p_new:
return self.target(centers, x_rand, region)
else:
while True:
x_candidate = [0, 0]
# parallel to one axis
line = np.random.randint(2, size=1)[0]
x_candidate[line] = x_rand[line]
# sample another component
r = round(1 / num_grid, 10)
x_candidate[1 - line] = round(
np.random.randint(num_grid, size=1)[0] * r + r / 2, 10)
if x_candidate != x_rand:
if 'o' in self.label(x_candidate):
continue
return tuple(x_candidate)
def target(self, centers, source, tg):
source = source
tg = centers[tg.split('_')[0]]
destination = shortest_path(self.env, tuple(source), tg)
return destination
def acpt_check(self, q_new, label_new, obs_check):
"""
check the accepting state in the patg leading to q_new
:param q_min:
:param q_new:
:return:
"""
if self.seg == 'pre':
if 'accept' in q_new[1]:
self.goals.append(q_new)
elif self.seg == 'suf':
if self.obs_check(self.init, q_new[0], label_new,
obs_check) and self.checkTranB(
q_new[1], label_new, self.init[1]):
self.goals.append(q_new)
def extend(self, q_new, prec_list, label_new, 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 }
:param: succ: list of successor of the root
:return: extending the tree
"""
# if q_new == ((0.825, 0.425), 'T1_S1'):
# print('df')
added = 0
cost = np.inf
q_min = ()
for pre in prec_list:
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.add_group(q_new)
self.tree.add_node(q_new, cost=cost, label=label_new)
self.tree.add_edge(q_min, q_new)
self.acpt_check(q_new, label_new, obs_check)
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)
for u, v, d in edges:
if d == 'forward':
self.tree.nodes[v][
'cost'] = self.tree.nodes[v]['cost'] - delta_c
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]
paths[i] = [self.path_cost(path), path]
if self.seg == 'suf':
paths[i] = [
self.path_cost(path) +
np.abs(path[-1][0][0] - self.init[0][0]) +
np.abs(path[-1][0][1] - self.init[0][1]),
path + [self.init]
]
return paths
def path_cost(self, path):
"""
calculate cost
:param path:
:return:
"""
cost = 0
for k in range(len(path) - 1):
cost += np.abs(path[k + 1][0][0] -
path[k][0][0]) + np.abs(path[k + 1][0][1] -
path[k][0][1])
return cost
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)
