def hpn(operators, current_state, goal, world, abs_info=None, maxdepth=np.inf, depth=0, tree=None):
'''Implements HPN (Hierarchical Planning in the Now) algorithm of Kaelbing and Lozano-Perez.
Args:
operators (list of Operator): Operators to use in the planning.
current_state (ConjunctionOfFluents): Current state, represeneted as a conjunction of fluents.
goal (ConjunctionOfFluents): Goal state, described as conjunction of fluents.
world: Defined by the domain; used to execute operator instances and get the new state of the world.
abs_info: (AbstractionInfo): Keeps track of abstraction level of the operators. Typically not passed in
at the top level call.
maxdepth (int): Max allowed depth of the planning hierarchy.
depth (int): Current depth of the planning hierarchy.
tree (HPlanTree): Data structure representing the hierarchical planning tree. Updated as this
function recurses, and can be used to visualize the resulting plan.
'''
if depth > maxdepth:
raise RuntimeError('Max recursion depth exceeded')
if abs_info is None:
abs_info = AbstractionInfo()
plan = a_star(
goal, # start from the goal and work backwards
lambda s: world.entails(s), # we are done when we reach the current state
lambda s: applicable_ops(operators, world, current_state, s, abs_info), # actions
lambda s: num_violated_fluents(world, s) # heuristic
)
if plan is None:
raise PlanningFailedError('A* could not find a plan')
plan.reverse()
tree.plan = []
for op, subgoal in plan:
tree.plan.append((op, HPlanTree(subgoal)))
for op, subtree in tree.plan:
subgoal = subtree.goal
if op is None:
pass
elif op.concrete:
print 'Executing:', op
current_state = world.execute(op)
else:
abs_info.inc_abs_level(op.target)
hpn(operators, current_state, subgoal, world, abs_info.copy(), maxdepth, depth+1, subtree)
def plan_flat(operators, world, current_state, goal):
'''Uses goal regression to plan without any hierarchy. Useful for testing.
'''
class ConcreteAbs:
def get_abs_level(self, f):
return np.inf
class ZeroAbs:
def get_abs_level(self, f):
return 0
plan = a_star(
goal, # start from the goal and work backwards
lambda s: current_state.entails(s), # we are done when we a state that is already true
lambda s: applicable_ops(operators, world, current_state, s, ConcreteAbs()), # actions
lambda s: num_violated_fluents(current_state, s) # heuristic
)
if plan == None:
return None
return list(reversed(plan))
n = 1000
conn_dist = 0.1
points = np.random.random((n, 2))
def action_generator(state):
for neighbor in range(len(points)):
d = linalg.norm(points[state] - points[neighbor])
if d < conn_dist:
yield neighbor, neighbor, d # action, next_state, cost
start = 0
goal = n-1
p = a_star.a_star(
start,
lambda s: s == goal,
action_generator,
lambda s: linalg.norm(points[goal] - points[s])
)
print p
plt.plot(points[:,0], points[:,1], 'b.')
plt.plot([points[start][0]], [points[start][1]], 'ro')
plt.plot([points[goal][0]], [points[goal][1]], 'go')
path_points = np.array([points[ii] for (ii, ii) in p])
plt.plot(path_points[:,0], path_points[:,1], 'm-o')
plt.show()