def do_operator(self, openset):
# updating for an operator.
assert(self.task[0] in operators and len(self.children) == 0)
if VERBOSE: print("ORNODE: ... cur_node {} is an OPERATOR".format(self))
operator = operators[self.task[0]]
new_state = operator(copy.deepcopy(self.before_state),*self.task[1:])
if new_state:
self.post_state = new_state
self.success = True
self.completed = True
# Set Cost
self.cost = self.state.cost_func(self.state, self.task)
self.cost_lower_bound = self.cost
# Continue depending on the lower-bound
if self.right != None: # Now consider adding the next or-node
add_right = False
if self.parent.success and self.parent.completed:
# now try to improve lowerbound
add_right = self.parent.report_lb_cost(self)
else:
# If parent is not complete, then add .right Node fo sho
add_right = True
if add_right:
if VERBOSE:
print("Adding RIGHT node to openset {}".format(self.right))
self.right.before_state = self.post_state
openset.append(self.right)
else:
if VERBOSE: print("PLANTREE.UPDATE: DECIDED NOT to add next-node: {} with parent {}".format(self.right, self.parent))
if VERBOSE: print("Found plan for node {} as: ".format(self))
if VERBOSE: print(self.get_plan())
else:
# This is a dead node (success = False)
if VERBOSE:
print("... ... FAILED operator {}; state:".format(self.task))
rrw.print_board(self.before_state)
self.post_state = False
self.success = False
self.completed = True
self.cost = sys.maxint
def seek_bb(state, tasks, verbose=0, all_plans=True):
if verbose:
print("SEEK_BB: solving problem for task {} and state:".format(tasks))
print("STATE:")
rrw.print_board(state)
print_state(state)
print("...")
print("VISITED: ", state.visited)
root = andNode(state, None, tasks)
openset = []
# Add top-level tasks to openset # AND
left_node = None
for task in tasks:
# Create node to represent task
task_node = orNode(state, root, [task])
if left_node != None:
task_node.left = left_node
left_node = task_node
# Add as part of AND children of root
root.children.append(task_node)
# Add to openset
openset.append(task_node)
best_cost = sys.maxint
# Iterate to decompose until openset is done.
while len(openset) != 0:
if root.success and not all_plans:
break
if verbose: print("root cost:{}".format(root.cost))
cur_node = openset.pop()
if verbose: print("Tasks: {}; Parent: {}; Cost: {}".format(cur_node.name, cur_node.parent, cur_node.cost))
if cur_node.tasks == []:
# Basecase
cur_node.success = True
cur_node.completed = True
cur_node.cost = 0
if verbose: print("right", cur_node.right)
if verbose: print("parent-right", cur_node.parent.right)
cur_node.parent.update(cur_node, openset)
continue
# Otherwise, decompose this node and add new nodes to queue
tasks = cur_node.tasks
if isinstance(cur_node, andNode):
if verbose: print("is decomposition")
left_node = None
for task in tasks:
child_node = orNode(cur_node.before_state, cur_node, [task])
cur_node.add_child(child_node)
openset.append(cur_node.children[0])
elif isinstance(cur_node, orNode):
task = tasks[0]
if task[0] in methods:
if verbose: print("is method")
relevant = methods[task[0]]
for method in relevant:
decompositions = method(cur_node.before_state, *task[1:]) # retruns the set of possible decomps
if verbose: print("decompositions: {}".format(decompositions))
if decompositions[0] == False:
# This node failed
cur_node.completed = True
cur_node.success = False
cur_node.parent.update(cur_node, openset)
continue
for sequence in decompositions:
child_node = andNode(cur_node.before_state, cur_node, sequence)
cur_node.add_child(child_node)
openset.append(cur_node.children[0])
elif task[0] in operators:
if verbose: print("is operators")
cur_node.update(None, openset)
if verbose: print("ACTIVESET: ", openset)
# raw_input("...")
# one_plan = root.get_plan()
if verbose:
print("root: ", root.get_string())
print("get_num_plans:", root.get_num_plans())
print("num_opt_plans:", root.get_num_opt_plans())
plans = root.get_all_opt_plans()
print("all opt plans: {}".format(plans))
# for p in plans:
# print("opt p", p[0])
# return [one_plan]
return root
def reconstruct_path(came_from, current):
to_return = [current]
while current in came_from:
current = came_from[current]
to_return.append(current)
to_return.reverse()
return to_return
def dist_between(current, neighbor):
return 1
VERBOSE = True
if __name__ == "__main__":
print("Testing a_star")
world = random_rovers_world.get_random_world(10, 10)
world.rand = False
print("Random world: ")
random_rovers_world.print_board(world)
# pyhop.print_state(world)
sink = random.choice(world.loc.keys())
print("*** Navigating from {} to {} ***".format(world.at["A1"], sink))
path = a_star(world, "A1", sink)
print(path)
for action in path:
print("{}\t{}".format(action, world.cost_func(world, action)))
