Beispiel #1
0
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)
Beispiel #2
0
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()