EAGER_TEST = True
DUMMY_CONSTRAINTS = (
    Identity,
    Unconstrained,
    Universe,
)
class ConstraintType(object):
class Constraint(object):
    def __init__(self, types=(), test=None, domain=(), satisfying=(), name=None, **kwargs):
    def __call__(self, *args, **kwargs):
    def __repr__(self):
    def __init__(self, constraint, values):
    def __eq__(self, other):
    def __ne__(self, other):
    def __hash__(self):
    def __repr__(self):
def convert_constraint(constraint):
def make_test_stream(constraint_form):
def make_con_constants(con): class VariableType(Type):
class FreeParameter(SSParameter):
class Variable(object):
class VariableSet(object):
class VariableMember(object):
    def __init__(self, name=None, domain=None):
    def __call__(self, value):
    def get_value(self, const):
    def param(self, name):
def get_value(const):
    def __init__(self, name, type=None):
def var_name(var):
def var_args(var):
def split_var(var):
    def __init__(self, name, dtype, args=[]):
    def __repr__(self):
    def __init__(self, name):
    def __getitem__(self, var):
    def __call__(self, *args):
    def __repr__(self):
    def __init__(self, temp, var):
    def __eq__(self, other):
    def __ne__(self, other):
    def __hash__(self):
    def __repr__(self):
def is_parameter(v):
def is_constant(v):
def expand_variables(vars):
def make_var_constants(var, var_map):
X = VariableSet('x')
U = VariableSet('u') class Clause(object):
    num = 0
    prefix = 'Clause%s'

    def __init__(self, constraints, name=None):
        for con in constraints:
            if not isinstance(con, Constraint):
                raise ValueError('%s must be a %s' %
                                 (con, Constraint.__name__))
        self.constraints = tuple(constraints)

        self.n = Clause.num
        Clause.num += 1
        self.name = name if name is not None else self.prefix % self.n

    def __repr__(self):

        return str(self.name) class Sampler(object):

    def __init__(self, values, gen, inputs=(), domain=(), name=None, **kwargs):
        self.cons = filter(lambda c: isinstance(c, Constraint), values)
        self.gen = gen
        self.inputs = tuple(inputs)
        self.domain = domain

        self.name = name if name is not None else self.__class__.__name__ + \
            str(values)
        self.kwargs = kwargs
        outputs = []
        for val in values:
            if isinstance(val, Constraint):
                for par in val.values:
                    assert isinstance(par, FreeParameter)
                    if par not in self.inputs and par not in outputs:
                        outputs.append(par)
            elif isinstance(val, FreeParameter):
                if val not in self.inputs and val not in outputs:
                    outputs.append(val)
            else:
                raise ValueError('Unexpected value %s' % val)
        self.outputs = tuple(outputs)

    def __repr__(self):
        return self.name class FTSProblem(object):

    def __init__(self, state_vars, control_vars, transition, samplers, initial_state, goal_constraints):
        self.state_vars = state_vars
        self.control_vars = control_vars
        self.transition = transition
        self.samplers = samplers
        self.initial_state = initial_state
        self.goal_constraints = goal_constraints

    def get_variable_map(self):
        return {var.name: var for var in self.state_vars + self.control_vars}

    def get_constraint_forms(self):
        con_forms = set()
        for con in self.initial_state + self.goal_constraints:
            if con.constraint != Eq:
                con_forms.add(con.constraint)
        for clause in self.transition:
            for con in clause.constraints:
                if con.constraint != Eq:
                    con_forms.add(con.constraint)
        for sampler in self.samplers:
            for con in list(sampler.cons) + list(sampler.domain):
                con_forms.add(con.constraint)
        return con_forms

    def __repr__(self):
        return 'Factored Transition System Problem\n'           'State Variables: %s\n'           'Control Variables: %s\n'           'Transition Relation: %s\n'           'Samplers: %s\n'           'Initial State: %s\n'           'Goal Constraints: %s' % (self.state_vars, self.control_vars,
                                                                                                                                                                                                                                                                 self.transition, self.samplers, self.initial_state, self.goal_constraints) def constraint_to_stripstream(fts_problem, do_axioms=True, test_streams=True):
    expanded_state_vars = expand_variables(fts_problem.state_vars)

    var_map = fts_problem.get_variable_map()
    axiom_map = {}
    initial_atoms = convert_initial_state(
        fts_problem.initial_state, var_map, expanded_state_vars)
    goal_literals = convert_goal_constraints(
        fts_problem.goal_constraints, var_map, axiom_map)

    actions, axioms = convert_transition(
        fts_problem.transition, var_map, axiom_map)
    cond_streams = map(convert_sampler, fts_problem.samplers)

    convert_constraint_forms(fts_problem.get_constraint_forms(), initial_atoms, cond_streams,
                             test_streams=test_streams)
    objects = convert_domains(
        fts_problem.state_vars + fts_problem.control_vars)

    return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, objects) def convert_plan(plan):
    if plan is None:
        return plan
    return [(action, map(get_value, args)) for action, args in plan] def rename_variables(assignments):
    for name, value in assignments.iteritems():
        if type(value) == VarType:
            value.name = name.lower()
        elif type(value) == ConstraintType:
            value.name = name.lower()
            value.predicate = Predicate(value.name, value.types)
        elif type(value) == FreeParameter:
            FreeParameter.__init__(value, name, value.type) 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 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
from stripstream.fts.constraint import Eq, ConType
from stripstream.fts.variable import VarType, Par, Var, X, U, nX
from stripstream.fts.clause import Clause
from stripstream.fts.sampler import Sampler
from stripstream.fts.problem import FTSProblem
from stripstream.fts.stripstream_conversion import constraint_to_stripstream
from stripstream.fts.utils import convert_plan, rename_variables
from stripstream.algorithms.search.fast_downward import get_fast_downward
from stripstream.algorithms.incremental.incremental_planner import incremental_planner
#!/usr/bin/env python2




def create_problem():
    """
    Creates the 1D task and motion planning FTSProblem problem.

    :return: a :class:`.FTSProblem`
    """

    blocks = ['block%i' % i for i in range(2)]
    num_poses = pow(10, 10)

    initial_config = 0  # the initial robot configuration is 0
    initial_poses = {block: i for i, block in enumerate(
        blocks)}  # the initial pose for block i is i

    # the goal pose for block i is i+1
    goal_poses = {block: i + 1 for i, block in enumerate(blocks)}

    ####################

    R_Q, B_P, B_H = 'R_Q', 'B_P', 'B_H'
    R_T = 'R_T'

    CONF = VarType()
    BOOL = VarType(domain=[True, False])
    POSE = VarType()
    BLOCK = VarType(domain=blocks)
    TRAJ = VarType()

    B, Q, P = Par(BLOCK), Par(CONF), Par(POSE)
    T, Q2 = Par(TRAJ), Par(CONF)

    LegalKin = ConType([POSE, CONF])
    CollisionFree = ConType([POSE, POSE], test=lambda p1,
                            p2: None in (p1, p2) or p1 != p2)
    Motion = ConType([CONF, TRAJ, CONF])

    rename_variables(locals())  # Trick to make debugging easier


    state_vars = [Var(R_Q, CONF), Var(B_P, POSE, args=[BLOCK]),