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 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
def update_cost_acc(multi_tree, subtree, succ, changed, index2node_root,
root2index, root_pred2index_node):
"""
update cost and acc of all children node in all subtrees
:param multi_tree:
:param subtree:
:param succ:
:param changed:
:param index2node_root:
:param root2index:
:param root_pred2index_node:
:return:
"""
# we only update the cost and acc if subtree connects to the root of the whole formula
if subtree.tree.nodes[succ]['cost'] < subtree.base:
# all other roots in the current tree or its subtrees
to_update = [root2index[succ]]
while to_update:
index = to_update.pop(0)
to_update += [root2index[r[1]] for r in index2node_root[index]]
root = multi_tree[index].init
multi_tree[index].tree.nodes[root]['acc'] = set(multi_tree[
root_pred2index_node[root][0]].tree.nodes[root]['acc'])
delta_c = multi_tree[index].tree.nodes[root]['cost'] - \
multi_tree[root_pred2index_node[root][0]].tree.nodes[root][
'cost']
for nodes in multi_tree[index].tree.nodes():
multi_tree[index].tree.nodes[nodes]['cost'] -= delta_c
if changed:
for u, v, d in dfs_labeled_edges(multi_tree[index].tree,
source=root):
if d == 'forward':
multi_tree[index].tree.nodes[v]['acc'] = set(
multi_tree[index].acpt_check(u, v)[0])
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
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 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 construction_tree_connect_root(transfer_tree, sample_list):
"""
construct the tree following the existing subpath
:param transfer_tree:
:param sample_list:
:return:
"""
# extend towards to other sample points
for k in range(1, len(sample_list)):
cand = sample_list[k]
# not in the set of nodes of the tree
cost = transfer_tree.tree.nodes[sample_list[k - 1]]['cost'] + \
np.linalg.norm(np.subtract(sample_list[k - 1][0], cand[0]))
if cand not in transfer_tree.tree.nodes:
# if 'accept' in cand[1]:
# transfer_tree.goals.append(cand)
label_cand = transfer_tree.label(cand[0])
transfer_tree.tree.add_node(cand, cost=cost, label=label_cand)
transfer_tree.tree.nodes[cand]['acc'] = transfer_tree.acpt_check(sample_list[k-1], cand)
transfer_tree.tree.add_edge(sample_list[k-1], cand)
transfer_tree.search_goal(cand, label_cand, transfer_tree.tree.nodes[cand]['acc'])
else:
delta_c = transfer_tree.tree.nodes[cand]['cost'] - cost
# update the cost of node in the transfer_tree rooted at sample_list[k]
if delta_c > 0:
if not list(transfer_tree.tree.pred[cand].keys()):
print('empty')
transfer_tree.tree.remove_edge(list(transfer_tree.tree.pred[cand].keys())[0], cand)
transfer_tree.tree.add_edge(sample_list[k-1], cand)
transfer_tree.tree.nodes[cand]['acc'] = transfer_tree.acpt_check(sample_list[k-1], cand)
edges = dfs_labeled_edges(transfer_tree.tree, source=cand)
for u, v, d in edges:
if d == 'forward':
# update cost
transfer_tree.tree.nodes[v]['cost'] = transfer_tree.tree.nodes[v]['cost'] - delta_c
# update accept state
transfer_tree.tree.nodes[v]['acc'] = transfer_tree.acpt_check(u, v)
def construction_tree_connect_sample(subtree, sample_list, multi_tree, init,
root_pred2index_node, root2index, centers,
todo_succ, connect, starting2waypoint,
subtask2path, newsubtask2subtask_p,
index2node_root):
"""
construct current subtree following the existing subpath
:param subtree:
:param sample_list:
:param multi_tree:
:param init:
:param root_pred2index_node:
:param root2index:
:return:
"""
# extend towards to other sample points
for k in range(1, len(sample_list)):
cand = sample_list[k]
# connect the second to last to other roots
if k == len(sample_list) - 1:
construction_tree_connect_root(
subtree, sample_list[k - 1],
subtree.tree.nodes[sample_list[k - 1]]['label'], centers,
todo_succ, connect, starting2waypoint, subtask2path,
newsubtask2subtask_p, root2index, root_pred2index_node,
index2node_root, multi_tree, init)
break
# not in the set of nodes of the tree
cost = subtree.tree.nodes[sample_list[k - 1]]['cost'] + \
np.linalg.norm(np.subtract(sample_list[k - 1][0], cand[0]))
if cand not in subtree.tree.nodes():
# extend-like
# if 'accept' in cand[1]:
# subtree.goals.append(cand)
label_cand = subtree.label(cand[0]) + '_' + str(1)
subtree.tree.add_node(cand, cost=cost, label=label_cand)
subtree.tree.nodes[cand]['acc'] = set(
subtree.acpt_check(sample_list[k - 1], cand)[0])
subtree.tree.add_edge(sample_list[k - 1], cand)
search_goal(multi_tree, init, subtree, cand, label_cand,
subtree.tree.nodes[cand]['acc'], root_pred2index_node,
root2index)
else:
# rewire-like
delta_c = subtree.tree.nodes[cand]['cost'] - cost
# update the cost of node in the subtree rooted at sample_list[k]
if delta_c > 0:
subtree.tree.remove_edge(
list(subtree.tree.pred[cand].keys())[0], cand)
subtree.tree.add_edge(sample_list[k - 1], cand)
acc, changed = subtree.acpt_check(sample_list[k - 1], cand)
subtree.tree.nodes[cand]['acc'] = set(acc)
edges = dfs_labeled_edges(subtree.tree, source=cand)
for u, v, d in edges:
if d == 'forward':
# update cost
subtree.tree.nodes[v][
'cost'] = subtree.tree.nodes[v]['cost'] - delta_c
# update accept state
if changed:
subtree.tree.nodes[v]['acc'] = set(
subtree.acpt_check(u, v)[0])
def construction_tree_connect_sample(subtree, sample_list, multi_tree, init,
root_pred2index_node, root2index, centers,
todo_succ, connect, starting2waypoint,
subtask2path, newsubtask2subtask_p,
index2node_root, obs_check):
"""
construct current subtree following the existing subpath
:param subtree:
:param sample_list:
:param multi_tree:
:param init:
:param root_pred2index_node:
:param root2index:
:return:
"""
# print('construction_tree_connect_sample')
# extend towards to other sample points
for k in range(1, len(sample_list) - 1):
cand = sample_list[k]
# not in the set of nodes of the tree
cost = subtree.tree.nodes[sample_list[k - 1]]['cost'] + \
np.abs(sample_list[k - 1][0][0] - cand[0][0]) + np.abs(sample_list[k - 1][0][1] - cand[0][1])
if cand not in subtree.tree.nodes():
# extend-like
# if 'accept' in cand[1]:
# subtree.goals.append(cand)
label_cand = subtree.label(cand[0]) + '_' + str(1)
subtree.tree.add_node(cand, cost=cost, label=label_cand)
subtree.tree.nodes[cand]['acc'] = set(
subtree.acpt_check(sample_list[k - 1], cand)[0])
subtree.tree.add_edge(sample_list[k - 1], cand)
search_goal(multi_tree, init, subtree, cand, label_cand,
subtree.tree.nodes[cand]['acc'], root_pred2index_node,
root2index, obs_check)
else:
# rewire-like
delta_c = subtree.tree.nodes[cand]['cost'] - cost
# update the cost of node in the subtree rooted at sample_list[k]
if delta_c > 0:
subtree.tree.remove_edge(
list(subtree.tree.pred[cand].keys())[0], cand)
subtree.tree.add_edge(sample_list[k - 1], cand)
acc, changed = subtree.acpt_check(sample_list[k - 1], cand)
subtree.tree.nodes[cand]['acc'] = set(acc)
edges = dfs_labeled_edges(subtree.tree, source=cand)
for u, v, d in edges:
if d == 'forward':
# update cost
subtree.tree.nodes[v][
'cost'] = subtree.tree.nodes[v]['cost'] - delta_c
# update accept state
if changed:
subtree.tree.nodes[v]['acc'] = set(
subtree.acpt_check(u, v)[0])
# those sampled point are more likely to be specific to one task, and the endpoint of the subpath may not
# directly connect to other roots since two regions can not be connected via a straight line
# connect to other roots
# connect the second to last to other roots
if k == len(sample_list) - 2:
construction_tree_connect_root(
subtree, sample_list[k],
subtree.tree.nodes[sample_list[k]]['label'], centers,
todo_succ, connect, starting2waypoint, subtask2path,
newsubtask2subtask_p, root2index, root_pred2index_node,
index2node_root, multi_tree, init, obs_check)