def get_states(universe, plan):
    instances = [operator.instantiate(parameters)
                 for operator, parameters in plan]
    constants = universe.type_to_objects
    axioms = list(instantiate_axioms(universe))
    state = universe.initial_atoms
    yield apply_axioms(state, constants, axioms)
    for i, instance in enumerate(instances):

        state = instance.apply(state, constants)
        yield apply_axioms(state, constants, axioms) def incremental_planner(problem, search=DEFAULT_SEARCH, max_time=INF, max_iterations=INF, max_calls=INF,
                        waves=True, frequency=1, optimal=False,
                        verbose=False, debug=False):
    """
    Incremental algorithm.

    :param problem: :class:`.STRIPStreamProblem`
    :param search: python function of the search subroutine
    :param max_time: numeric maximum planning time
    :param max_iterations: int maximum subroutine calls
    :param waves: boolean flag when ``True`` considers each stream evaluation iteration to be the current queue rather than a single stream
    :param frequency: numeric number of evaluation iterations given by ``waves`` (``float('inf')`` implies to evaluate all streams per iteration)
    :param optimal: boolean flag which saves the best current solution
    :param verbose: boolean flag which toggles the print output
    :param debug: boolean flag which prints the objects on each iteration
    :return: a sequence of :class:`.Action` and tuple of :class:`.Constant` pairs as well as :class:`.Universe`
    """

    universe = Universe(problem, use_ground=False, verbose=verbose)

    queue = deque()
    add_streams(universe, queue)
    best_plan, best_cost = None, INF
    for _ in irange(0, max_iterations):
        print 'Iteration: %d | Time: %s | Calls: %s | Search Time: %s | Solved: %s | Cost: %s' % (universe.iterations,
                                                                                                  round(
                                                                                                      time() - universe.start_time, 3), universe.calls, round(universe.search_time, 3),
                                                                                                  best_plan is not None, best_cost)
        if debug:
            for type in universe.type_to_objects:
                print type, len(universe.type_to_objects[type]), map(get_value, universe.type_to_objects[type])[:10]
            raw_input('Continue?\n')
        universe.iterations += 1
        t0 = time()
        plan = search(universe, max_time - (time() -
                                            universe.start_time), best_cost)
        universe.search_time += time() - t0
        if plan is not None:
            cost = plan_cost(universe, plan)
            if cost < best_cost:
                best_plan, best_cost = plan, cost
                if verbose:
                    print best_cost, convert_plan(best_plan)
            if not optimal:
                break
        if waves and not call_queue_wave(queue, universe, frequency, max_time, max_calls):
            break
        if not waves and not call_queue(queue, universe, frequency, max_time, max_calls):
            break
    if verbose:
        universe.print_statistics()
        if best_plan is not None:
            print_plan_stats(best_plan, universe)
            print 'Cost:', best_cost
            print 'Length:', len(best_plan)

    return best_plan, universe def simple_focused(problem, search=DEFAULT_SEARCH, max_time=INF, max_iterations=INF,
                   optimal=False, stream_cost=10, check_feasible=False, max_level=0,
                   greedy=True, shared=True, dfs=True,
                   verbose=False, debug=False):

    universe = Universe(problem, use_ground=False,
                        make_stream_instances=True, make_action_instances=True)

    initialize_universe(universe)
    universe.action_to_function = get_stream_functions(
        universe) if stream_cost is not None else {}

    for _ in irange(0, max_iterations):
        if verbose:
            print
        print_status(universe)
        if time() - universe.start_time >= max_time:
            break
        universe.iterations += 1
        if debug:
            focused_debug(universe)

        if check_feasible:
            plan = solve_real(universe, search, max_time)
            if plan is not None:
                assert is_real_solution(universe, plan)
                return plan, universe

        make_streams(universe, max_level)
        if debug:
            abstract_focused_debug(universe)

        atom_nodes = determine_reachable(universe)
        for instance in universe.action_instances:
            if isinstance(instance, ActionInstance):
                add_stream_cost(universe, atom_nodes, instance, stream_cost)
        plan, goals = solve_abstract(
            universe, atom_nodes, search, max_time, stream_cost)
        if verbose:
            print 'Cost:', plan_cost(universe, plan), 'Plan:', convert_plan(plan)
            raw_input('Continue?')
        if plan is None:
            if not universe.temp_blocked:
                break
            universe.temp_blocked = set()
            universe.resets += 1
            continue

        constants = set_union(set(args) for _, args in plan)
        abstract_constants = filter(
            lambda c: isinstance(c, AbstractConstant), constants)
        new_goals = goals.copy()
        new_goals.update(map(Concrete, abstract_constants))
        order = extract_streams(atom_nodes, new_goals)

        if verbose:
            print 'Goals:', len(goals)
            print 'Abstract constants:', len(abstract_constants)
            print 'Order:', order

        bound_plan = produce_bindings(
            universe, order, plan, greedy, shared, dfs)
        if verbose:
            print 'Bound plan:', bound_plan
        if bound_plan is not None:
            assert is_real_solution(universe, bound_plan)
            return bound_plan, universe
    return None, universe def get_bfs(max_time=30, max_cost=INF, verbose=False):
    """
    Returns a breadth-first search (BFS) search function configured using the arguments.

    :param max_time: a numeric constant for the maximum BFS search time.
    :param max_cost: a numeric constant for the maximum BFS solution cost.
    :param verbose: a boolean flag toggling the amount of terminal output
    :return: function wrapper around :class:`.bfs`
    """
    return lambda p, mt, mc: bfs(p, min(mt, max_time), min(mc, max_cost), verbose) def get_fast_downward(config='wastar2', max_time=30, max_cost=INF, verbose=False):
    """
    Returns a FastDownward (FD) search function configured using the arguments.

    :param config: a string that is a key of :data:`.search_options` giving the FD configuration.
    :param max_time: a numeric constant for the maximum FD search time.
    :param max_cost: a numeric constant for the maximum FD solution cost.
    :param verbose: a boolean flag toggling the amount of terminal output
    :return: function wrapper around :func:`.fast_downward`
    """
    return lambda p, mt, mc: fast_downward(p, config, min(mt, max_time), min(mc, max_cost), verbose) def convert_plan(plan):
    if plan is None:
        return plan
    return [(action, map(get_value, args)) for action, args in plan] SEPARATOR = '\n' + 85 * '-' + '\n' DEFAULT = 'default'
BFS = 'bfs'
FAST_DOWNWARD = 'fd'
FAST_FORWARD = 'ff'
LAPKT = 'lapkt'
PYPLANNERS = 'pyplanners'
INCREMENTAL, FOCUSED = 'incremental', 'focused'
DEFAULT_SEARCH_NAME = BFS
    DEFAULT_SEARCH_NAME = FAST_DOWNWARD
    DEFAULT_SEARCH_NAME = FAST_FORWARD
    DEFAULT_SEARCH_NAME = LAPKT
    DEFAULT_SEARCH = get_fast_downward()
    DEFAULT_SEARCH = get_fast_forward()
    DEFAULT_SEARCH = get_lakpt()
    DEFAULT_SEARCH = get_bfs() def get_lakpt(search='agl_bfs_f', verbose=False):
    """
    Returns a FastDownward (FD) search function configured using the arguments.

    :param search: name of a LAPKT search algorithm
    :param verbose: a boolean flag toggling the amount of terminal output
    :return: function wrapper around :func:`.fast_downward`
    """
    return lambda p, mt, mc: lapkt(p, search, verbose) def get_fast_forward(derived=True, verbose=False):
    """
    Returns a FastDownward (FD) search function configured using the arguments.

    :param derived: a boolean flag toggling the use of derived predicates
    :param verbose: a boolean flag toggling the amount of terminal output
    :return: function wrapper around :func:`.fast_downward`
    """
    return lambda p, mt, mc: fast_forward(p, derived, verbose) def get_grasp_problem(p=100):
    initial_config = 0
    initial_holding = False
    initial_poses = {'block1': p}
    goal_config = None
    goal_holding = 'block1'
    goal_poses = {}
    return TMPProblem(initial_config, initial_holding, initial_poses, goal_config, goal_holding, goal_poses) def get_distract_problem(p=100):
    initial_config = 0
    initial_holding = False
    initial_poses = [('block%s' % p, p) for p in range(1, p + 1)]
    goal_config = None
    goal_holding = initial_poses[-1][0]
    goal_poses = {}
    return TMPProblem(initial_config, initial_holding, dict(initial_poses), goal_config, goal_holding, goal_poses) def get_invert_problem(p=3):
    blocks = ['block%s' % p for p in range(0, p)]
    initial_config = 0
    initial_holding = False
    initial_poses = [(block, p) for p, block in enumerate(blocks)]
    goal_config = None
    goal_holding = None
    goal_poses = {block: len(blocks) - p - 1 for p, block in enumerate(blocks)}
    return TMPProblem(initial_config, initial_holding, dict(initial_poses), goal_config, goal_holding, goal_poses) def get_shift_problem(p=3):
    blocks = ['block%s' % p for p in range(0, p)]
    initial_config = 0
    initial_holding = False
    initial_poses = [(block, p) for p, block in enumerate(blocks)]
    goal_config = None
    goal_holding = None
    goal_poses = {block: p + 1 for p, block in enumerate(blocks)}
    return TMPProblem(initial_config, initial_holding, dict(initial_poses), goal_config, goal_holding, goal_poses) NUM_POSES = INF def compile_problem(tamp_problem):
    """
    Constructs a STRIPStream problem for the countable TMP problem.

    :param tamp_problem: a :class:`.TMPProblem`
    :return: a :class:`.STRIPStreamProblem`
    """

    B1, B2 = Param(BLOCK), Param(BLOCK)
    P1, P2 = Param(POSE), Param(POSE)
    Q1, Q2 = Param(CONF), Param(CONF)

    actions = [
        Action(name='pick', parameters=[B1, P1, Q1],
               condition=And(AtPose(B1, P1), HandEmpty(),
                             AtConf(Q1), LegalKin(P1, Q1)),
               effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
        Action(name='place', parameters=[B1, P1, Q1],
               condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),

                             ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),

               effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
        Action(name='move', parameters=[Q1, Q2],
               condition=AtConf(Q1),
               effect=And(AtConf(Q2), Not(AtConf(Q1)))),
    ]

    axioms = [

        Axiom(effect=Safe(B2, P1), condition=Exists(
            [P2], And(AtPose(B2, P2), CollisionFree(P1, P2)))),
    ]

    cond_streams = [



        EasyGenStream(inputs=[], outputs=[P1], conditions=[], effects=[],
                      generator=lambda: irange(0, NUM_POSES)),
        EasyGenStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
                      generator=lambda p: iter([p])),


        EasyTestStream(inputs=[P1, P2], conditions=[], effects=[CollisionFree(P1, P2)],
                       test=lambda p1, p2: p1 != p2, eager=EAGER_TESTS),
    ]

    constants = []

    initial_atoms = [
        AtConf(tamp_problem.initial_config),
    ] + [
        AtPose(block, pose) for block, pose in tamp_problem.initial_poses.iteritems()
    ]
    if tamp_problem.initial_holding is False:
        initial_atoms.append(HandEmpty())
    else:
        initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))

    goal_literals = []
    if tamp_problem.goal_holding is False:
        goal_literals.append(HandEmpty())
    elif tamp_problem.goal_holding is not None:
        goal_literals.append(Holding(tamp_problem.goal_holding))
    for block, goal in tamp_problem.goal_poses.iteritems():
        goal_literals.append(AtPose(block, goal))

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants) def visualize_atoms(viewer, atoms):
    conf = get_config(atoms)
    holding = get_holding(atoms)
    viewer.draw_robot(ROBOT_ROW, conf)
    if holding is not None:
        viewer.draw_block(ROBOT_ROW, conf, color=get_color(holding))
    for atom in atoms:
        if isinstance(atom, Atom) and atom.predicate == AtPose:
            b, p = atom.args
            viewer.draw_block(0, get_value(p), color=get_color(get_value(b)))
from time import time
from stripstream.algorithms.plan import get_states
from stripstream.algorithms.incremental.incremental_planner import incremental_planner
from stripstream.algorithms.focused.simple_focused import simple_focused
from stripstream.algorithms.search.bfs import get_bfs
from stripstream.algorithms.search.fast_downward import get_fast_downward
from stripstream.pddl.utils import convert_plan
from stripstream.utils import SEPARATOR
from stripstream.algorithms.utils import DEFAULT, BFS, FAST_DOWNWARD, INCREMENTAL, \
    FOCUSED, LAPKT, FAST_FORWARD, PYPLANNERS, DEFAULT_SEARCH
from stripstream.algorithms.search.lapkt import get_lakpt
from stripstream.algorithms.search.fast_forward import get_fast_forward
from stripstream.pddl.examples.countable_tamp.countable_tamp_utils import get_grasp_problem, \
    get_distract_problem, get_invert_problem, get_shift_problem, NUM_POSES
from stripstream.pddl.examples.countable_tamp.countable_tamp import compile_problem, visualize_atoms
        from stripstream.pddl.examples.countable_tamp.countable_tamp_viewer import CountableTMPViewer
            from stripstream.pddl.examples.countable_tamp.countable_tamp_viewer import CountableTMPViewer
import argparse
#!/usr/bin/env python


#from stripstream.algorithms.focused.focused_planner import focused_planner
#from stripstream.algorithms.hierarchy.replan import replan_hierarchy, first_selector
#from stripstream.algorithms.experimental.state_space import progression

#from stripstream.algorithms.search.pyplanners import get_pyplanners

#from stripstream.pddl.examples.countable_tamp.old_countable_tamp import compile_problem, visualize_atoms
#from stripstream.pddl.examples.countable_tamp.hierarchical_countable_tamp import compile_problem, visualize_atoms, NUM_POSES


# TODO - do a version of this without axioms
# TODO - do a version of this which includes objects in actions
# NOTE - need to handle conditional effects as conditions...
# Alternatively can just compile out axioms as currently doing and compare


def solve_countable_tamp(planner, search, visualize, display, verbose=False):
    # tamp_problem = get_grasp_problem(p=3) # 10 | 1000
    # tamp_problem = get_distract_problem(p=3) # 10 | 1000
    #tamp_problem = get_invert_problem(p=3)
    tamp_problem = get_shift_problem(p=3)
    #tamp_problem = get_distract_place_problem(p=40)

    # stream_problem = compile_problem(tamp_problem,
    # stream_fn=generative_streams) # constant_streams | implicit_streams |
    # constant_streams
    stream_problem = compile_problem(tamp_problem)
    print stream_problem
    #stream_problem.operators = stream_problem.convert_axioms_to_effects()
    # stream_problem.replace_axioms()
    # for action in stream_problem.operators:
    #  print action.pddl(True)

    viewer = None
    if visualize:
        viewer = CountableTMPViewer(1, NUM_POSES, title='State', height=250)
        visualize_atoms(viewer, stream_problem.initial_atoms)
        raw_input('Continue?')

    verbose_search = False
    if search == DEFAULT:
        search_fn = DEFAULT_SEARCH

    elif search == BFS: