def phase_two(problem, results):
print "Phase 2 solving SCC sub problems"
solved = True
num_comp, scc, c, paths = results
solution = [None]*len(scc)
for i,sc in enumerate(scc):
sc = list(sc)
if len(sc) ==1:
solution[i] = (sc, paths[sc[0]])
if len(sc) > 1:
if len(sc) >= 3:
print "sc is too long. won't attempt to find path"
solved = False
break
print "trying to resolve sub problem for %s "% sc
start,goalTest,heuristic,goals =problem.getSearchParams(sc)
obstacles = genSubProblemObs(problem, scc, i)
print start, goals, obstacles
path, expanded, Cost = ucSearch(successors(\
problem.wm,False, obstacles, problem.alpha), start,goalTest,heuristic)
if path == None:
print "failed to solve sub problem."
solved = False
break
else:
solution[i] = (sc, path)
if not solved:
return None
else:
return solution
def get_full_config_path(problem): # full config space plan: start,goalTest,heuristic,_ = problem.getSearchParams(problem.bots) path, expanded, Cost = ucSearch(successors(problem.wm,False, obstacles), start,goalTest,heuristic) print "full config space plan. expanded nodes = %s , path length =%s "\ % (expanded, len(path)) return path
def phase_three(problem, results):
print "Phase 3 solving SCC full problems"
num_comp, scc, c, paths = results
solution = [None]*len(scc)
for i,sc in enumerate(scc):
sc = list(sc)
if len(sc) >1 :
if len(sc) >= 3:
print "sc is too long. won't attempt to find path"
return None
print "trying to find composite path for %s " % sc
start,goalTest,heuristic,goals =problem.getSearchParams(sc)
path, expanded, Cost = ucSearch(successors(\
problem.wm,False), start,goalTest,heuristic)
if path == None:
print "no solution found"
return None
else:
print "Path found. need to reorder constraint graph!"
for n in sc: c.remove_node(n) # remove composite nodes
c.add_node(sc[0], name=sc) # only add first node back
# get constraints
constraints = constraints_for_path(problem, path,sc)
print constraints
c.add_edges_from(constraints)
paths[sc[0]] = path
num_comp, scc, max_sc = getSCC(c)
solution = [None]*len(scc)
for i,sc in enumerate(scc):
sc = list(sc)
if 'name' in c.node[sc[0]]:
solution[i] = (c.node[sc[0]]['name'], paths[sc[0]])
else:
solution[i] = (sc, paths[sc[0]])
if max_sc == 1:
print "solved problem!"
return solution
else:
print "composite planner did not decrease size of SCC to 1"
return None
