def rrt_connect(q1, q2, distance, sample, extend, collision, iterations=RRT_ITERATIONS):
if collision(q1) or collision(q2): return None
root1, root2 = TreeNode(q1), TreeNode(q2)
nodes1, nodes2 = [root1], [root2]
for i in irange(iterations):
if len(nodes1) > len(nodes2):
nodes1, nodes2 = nodes2, nodes1
s = sample()
last1 = argmin(lambda n: distance(n.config, s), nodes1)
for q in extend(last1.config, s):
if collision(q): break
last1 = TreeNode(q, parent=last1)
nodes1.append(last1)
last2 = argmin(lambda n: distance(n.config, last1.config), nodes2)
for q in extend(last2.config, last1.config):
if collision(q): break
last2 = TreeNode(q, parent=last2)
nodes2.append(last2)
else:
path1, path2 = last1.retrace(), last2.retrace()
if path1[0] != root1:
path1, path2 = path2, path1
return configs(path1[:-1] + path2[::-1])
return None
def extract_relaxed_plan(goal, operator_costs, variable_costs, tree):
operator_layers = get_layers(operator_costs)
num_goal_layers = operator_costs[goal].level + 1
goals = [{var: set() for var in variable_costs.keys()} for _ in range(num_goal_layers)]
plan = [set() for _ in range(num_goal_layers - 1)]
marked = [{var: set() for var in variable_costs.keys()} for _ in range(num_goal_layers)]
easiest_fn = lambda o: EASIEST_HEURISTIC(o, operator_costs, variable_costs, goals, get_conditions(o, tree))
for var, value in goal.conditions.iteritems():
goals[variable_costs[var][value].level][var].add(value)
for level in reversed(range(1, num_goal_layers)):
for var, value in LAYER_ORDER(variable_costs, goals[level]):
if value in marked[level][var]: continue
if ALL_BACK_POINTERS:
easiest_operator = argmin(easiest_fn, (o for o, p in operator_costs.iteritems()
if p.level < level and var in o.effects and o.effects[var] == value))
else:
easiest_operator = argmin(easiest_fn, (o for o in operator_layers[level-1]
if var in o.effects and o.effects[var] == value)) # NOTE - compares cost and level
plan[level-1].add(easiest_operator)
for var2, value2 in get_conditions(easiest_operator, tree):
if value2 not in marked[level-1][var2]:
goals[variable_costs[var2][value2].level][var2].add(value2)
for var2, value2 in easiest_operator.effects.iteritems():
marked[level][var2].add(value2)
marked[level-1][var2].add(value2) # Assumes that actions are read off in order they are selected (no need to reachieve)
return plan, goals
def rrt(start, goal_sample, distance, sample, extend, collision, goal_test=lambda q: False, iterations=RRT_ITERATIONS, goal_probability=.2):
# start - start configuration
# goal - goal configuration
# sample - sampling function
# extend - extend function
# collision - collision checking fn
if collision(start): return None
if not callable(goal_sample):
g = goal_sample
goal_sample = lambda: g
nodes = [TreeNode(start)]
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else sample()
last = argmin(lambda n: distance(n.config, s), nodes)
for q in extend(last.config, s):
if collision(q): break
last = TreeNode(q, parent=last)
nodes.append(last)
if goal_test(last.config): return configs(last.retrace())
else:
if goal: return configs(last.retrace())
return None
def grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500):
if goal in self: self[goal].retrace()
if self.collision(goal): return None
nodes1, new_nodes1 = list(take(randomize(self.nodes.values()), max_tree_size)), []
nodes2, new_nodes2 = [], [TreeNode(goal)]
for _ in irange(iterations):
if len(nodes1) + len(new_nodes1) > len(nodes2) + len(new_nodes2):
nodes1, nodes2 = nodes2, nodes1
new_nodes1, new_nodes2 = new_nodes2, new_nodes1
s = self.sample()
last1 = argmin(lambda n: self.distance(n.config, s), nodes1 + new_nodes1)
for q in self.extend(last1.config, s):
if self.collision(q): break
last1 = TreeNode(q, parent=last1)
new_nodes1.append(last1)
last2 = argmin(lambda n: self.distance(n.config, last1.config), nodes2 + new_nodes2)
for q in self.extend(last2.config, last1.config):
if self.collision(q): break
last2 = TreeNode(q, parent=last2)
new_nodes2.append(last2)
else:
if len(nodes1) == 0:
nodes1, nodes2 = nodes2, nodes1
new_nodes1, new_nodes2 = new_nodes2, new_nodes1
last1, last2 = last2, last1
path1, path2 = last1.retrace(), last2.retrace()[:-1][::-1]
for p, n in pairs(path2): n.parent = p
if len(path2) == 0: # TODO - still some kind of circular error
for n in new_nodes2:
if n.parent == last2:
n.parent = path1[-1]
else:
path2[0].parent = path1[-1]
path = path1 + path2
if store in [ts.ALL, ts.SUCCESS]:
self.add(*(new_nodes1 + new_nodes2[:-1]))
elif store == ts.PATH:
new_nodes_set = set(new_nodes1 + new_nodes2[:-1])
self.add(*[n for n in path if n in new_nodes_set])
return path
if store == ts.ALL:
self.add(*new_nodes1)
return None
def grow(self, goal_sample, iterations=50, goal_probability=.2, store=ts.PATH, max_tree_size=500):
if not callable(goal_sample): goal_sample = lambda: goal_sample
nodes, new_nodes = list(take(randomize(self.nodes.values()), max_tree_size)), []
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else self.sample()
last = argmin(lambda n: self.distance(n.config, s), nodes + new_nodes)
for q in self.extend(last.config, s):
if self.collision(q): break
last = TreeNode(q, parent=last)
new_nodes.append(last)
else:
if goal:
path = last.retrace()
if store in [ts.ALL, ts.SUCCESS]:
self.add(*new_nodes)
elif store == ts.PATH:
new_nodes_set = set(new_nodes)
self.add(*[n for n in path if n in new_nodes_set])
return path
if store == ts.ALL:
self.add(*new_nodes)
return None
